1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16 package com.irurueta.navigation.inertial.calibration.accelerometer;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20 import com.irurueta.navigation.LockedException;
21 import com.irurueta.navigation.NotReadyException;
22 import com.irurueta.navigation.frames.CoordinateTransformation;
23 import com.irurueta.navigation.frames.ECEFPosition;
24 import com.irurueta.navigation.frames.ECEFVelocity;
25 import com.irurueta.navigation.frames.FrameType;
26 import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
27 import com.irurueta.navigation.frames.NEDFrame;
28 import com.irurueta.navigation.frames.NEDPosition;
29 import com.irurueta.navigation.frames.NEDVelocity;
30 import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
31 import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
32 import com.irurueta.navigation.inertial.calibration.AccelerationTriad;
33 import com.irurueta.navigation.inertial.calibration.BodyKinematicsGenerator;
34 import com.irurueta.navigation.inertial.calibration.CalibrationException;
35 import com.irurueta.navigation.inertial.calibration.IMUErrors;
36 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
37 import com.irurueta.navigation.inertial.estimators.ECEFGravityEstimator;
38 import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
39 import com.irurueta.statistics.UniformRandomizer;
40 import com.irurueta.units.Acceleration;
41 import com.irurueta.units.AccelerationUnit;
42 import org.junit.jupiter.api.Test;
43
44 import java.util.ArrayList;
45 import java.util.Collections;
46 import java.util.Random;
47
48 import static org.junit.jupiter.api.Assertions.*;
49
50 class KnownBiasAndGravityNormAccelerometerCalibratorTest implements
51 KnownBiasAndGravityNormAccelerometerCalibratorListener {
52
53 private static final double TIME_INTERVAL_SECONDS = 0.02;
54
55 private static final double MICRO_G_TO_METERS_PER_SECOND_SQUARED = 9.80665E-6;
56 private static final double DEG_TO_RAD = 0.01745329252;
57
58 private static final double MIN_ANGLE_DEGREES = -180.0;
59 private static final double MAX_ANGLE_DEGREES = 180.0;
60
61 private static final double MIN_LATITUDE_DEGREES = -90.0;
62 private static final double MAX_LATITUDE_DEGREES = 90.0;
63 private static final double MIN_LONGITUDE_DEGREES = -180.0;
64 private static final double MAX_LONGITUDE_DEGREES = 180.0;
65 private static final double MIN_HEIGHT = -50.0;
66 private static final double MAX_HEIGHT = 50.0;
67
68 private static final int LARGE_MEASUREMENT_NUMBER = 70000;
69
70 private static final double ABSOLUTE_ERROR = 1e-8;
71 private static final double LARGE_ABSOLUTE_ERROR = 5e-5;
72
73 private static final int TIMES = 70;
74
75 private int calibrateStart;
76 private int calibrateEnd;
77
78 @Test
79 void testConstructor1() throws WrongSizeException {
80 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
81
82
83 assertEquals(0.0, calibrator.getBiasX(), 0.0);
84 assertEquals(0.0, calibrator.getBiasY(), 0.0);
85 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
86 final var bx1 = calibrator.getBiasXAsAcceleration();
87 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
88 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
89 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
90 calibrator.getBiasXAsAcceleration(bx2);
91 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
92 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
93 final var by1 = calibrator.getBiasYAsAcceleration();
94 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
95 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
96 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
97 calibrator.getBiasYAsAcceleration(by2);
98 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
99 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
100 final var bz1 = calibrator.getBiasZAsAcceleration();
101 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
102 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
103 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
104 calibrator.getBiasZAsAcceleration(bz2);
105 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
106 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
107 final var biasTriad1 = calibrator.getBiasAsTriad();
108 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
109 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
110 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
111 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
112 final var biasTriad2 = new AccelerationTriad();
113 calibrator.getBiasAsTriad(biasTriad2);
114 assertEquals(biasTriad1, biasTriad2);
115 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
116 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
117 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
118 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
119 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
120 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
121 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
122 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
123 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
124 final var bias1 = calibrator.getBias();
125 assertArrayEquals(new double[3], bias1, 0.0);
126 final var bias2 = new double[3];
127 calibrator.getBias(bias2);
128 assertArrayEquals(bias1, bias2, 0.0);
129 final var b1 = calibrator.getBiasAsMatrix();
130 assertEquals(new Matrix(3, 1), b1);
131 final var b2 = new Matrix(3, 1);
132 calibrator.getBiasAsMatrix(b2);
133 assertEquals(b1, b2);
134 final var ma1 = calibrator.getInitialMa();
135 assertEquals(new Matrix(3, 3), ma1);
136 final var ma2 = new Matrix(3, 3);
137 calibrator.getInitialMa(ma2);
138 assertEquals(ma1, ma2);
139 assertNull(calibrator.getMeasurements());
140 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
141 calibrator.getMeasurementType());
142 assertFalse(calibrator.isOrderedMeasurementsRequired());
143 assertFalse(calibrator.isQualityScoresRequired());
144 assertFalse(calibrator.isCommonAxisUsed());
145 assertNull(calibrator.getListener());
146 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
147 assertFalse(calibrator.isReady());
148 assertFalse(calibrator.isRunning());
149 assertNull(calibrator.getEstimatedMa());
150 assertNull(calibrator.getEstimatedSx());
151 assertNull(calibrator.getEstimatedSy());
152 assertNull(calibrator.getEstimatedSz());
153 assertNull(calibrator.getEstimatedMxy());
154 assertNull(calibrator.getEstimatedMxz());
155 assertNull(calibrator.getEstimatedMyx());
156 assertNull(calibrator.getEstimatedMyz());
157 assertNull(calibrator.getEstimatedMzx());
158 assertNull(calibrator.getEstimatedMzy());
159 assertNull(calibrator.getEstimatedCovariance());
160 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
161 assertNull(calibrator.getGroundTruthGravityNorm());
162 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
163 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
164 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
165 }
166
167 @Test
168 void testConstructor2() throws WrongSizeException {
169 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(this);
170
171
172 assertEquals(0.0, calibrator.getBiasX(), 0.0);
173 assertEquals(0.0, calibrator.getBiasY(), 0.0);
174 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
175 final var bx1 = calibrator.getBiasXAsAcceleration();
176 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
177 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
178 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
179 calibrator.getBiasXAsAcceleration(bx2);
180 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
181 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
182 final var by1 = calibrator.getBiasYAsAcceleration();
183 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
184 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
185 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
186 calibrator.getBiasYAsAcceleration(by2);
187 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
188 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
189 final var bz1 = calibrator.getBiasZAsAcceleration();
190 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
191 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
192 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
193 calibrator.getBiasZAsAcceleration(bz2);
194 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
195 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
196 final var biasTriad1 = calibrator.getBiasAsTriad();
197 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
198 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
199 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
200 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
201 final var biasTriad2 = new AccelerationTriad();
202 calibrator.getBiasAsTriad(biasTriad2);
203 assertEquals(biasTriad1, biasTriad2);
204 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
205 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
206 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
207 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
208 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
209 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
210 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
211 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
212 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
213 final var bias1 = calibrator.getBias();
214 assertArrayEquals(new double[3], bias1, 0.0);
215 final var bias2 = new double[3];
216 calibrator.getBias(bias2);
217 assertArrayEquals(bias1, bias2, 0.0);
218 final var b1 = calibrator.getBiasAsMatrix();
219 assertEquals(new Matrix(3, 1), b1);
220 final var b2 = new Matrix(3, 1);
221 calibrator.getBiasAsMatrix(b2);
222 assertEquals(b1, b2);
223 final var ma1 = calibrator.getInitialMa();
224 assertEquals(new Matrix(3, 3), ma1);
225 final var ma2 = new Matrix(3, 3);
226 calibrator.getInitialMa(ma2);
227 assertEquals(ma1, ma2);
228 assertNull(calibrator.getMeasurements());
229 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
230 calibrator.getMeasurementType());
231 assertFalse(calibrator.isOrderedMeasurementsRequired());
232 assertFalse(calibrator.isQualityScoresRequired());
233 assertFalse(calibrator.isCommonAxisUsed());
234 assertSame(this, calibrator.getListener());
235 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
236 assertFalse(calibrator.isReady());
237 assertFalse(calibrator.isRunning());
238 assertNull(calibrator.getEstimatedMa());
239 assertNull(calibrator.getEstimatedSx());
240 assertNull(calibrator.getEstimatedSy());
241 assertNull(calibrator.getEstimatedSz());
242 assertNull(calibrator.getEstimatedMxy());
243 assertNull(calibrator.getEstimatedMxz());
244 assertNull(calibrator.getEstimatedMyx());
245 assertNull(calibrator.getEstimatedMyz());
246 assertNull(calibrator.getEstimatedMzx());
247 assertNull(calibrator.getEstimatedMzy());
248 assertNull(calibrator.getEstimatedCovariance());
249 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
250 assertNull(calibrator.getGroundTruthGravityNorm());
251 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
252 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
253 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
254 }
255
256 @Test
257 void testConstructor3() throws WrongSizeException {
258 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
259 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements);
260
261
262 assertEquals(0.0, calibrator.getBiasX(), 0.0);
263 assertEquals(0.0, calibrator.getBiasY(), 0.0);
264 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
265 final var bx1 = calibrator.getBiasXAsAcceleration();
266 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
267 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
268 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
269 calibrator.getBiasXAsAcceleration(bx2);
270 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
271 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
272 final var by1 = calibrator.getBiasYAsAcceleration();
273 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
274 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
275 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
276 calibrator.getBiasYAsAcceleration(by2);
277 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
278 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
279 final var bz1 = calibrator.getBiasZAsAcceleration();
280 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
281 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
282 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
283 calibrator.getBiasZAsAcceleration(bz2);
284 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
285 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
286 final var biasTriad1 = calibrator.getBiasAsTriad();
287 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
288 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
289 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
290 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
291 final var biasTriad2 = new AccelerationTriad();
292 calibrator.getBiasAsTriad(biasTriad2);
293 assertEquals(biasTriad1, biasTriad2);
294 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
295 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
296 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
297 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
298 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
299 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
300 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
301 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
302 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
303 final var bias1 = calibrator.getBias();
304 assertArrayEquals(new double[3], bias1, 0.0);
305 final var bias2 = new double[3];
306 calibrator.getBias(bias2);
307 assertArrayEquals(bias1, bias2, 0.0);
308 final var b1 = calibrator.getBiasAsMatrix();
309 assertEquals(new Matrix(3, 1), b1);
310 final var b2 = new Matrix(3, 1);
311 calibrator.getBiasAsMatrix(b2);
312 assertEquals(b1, b2);
313 final var ma1 = calibrator.getInitialMa();
314 assertEquals(new Matrix(3, 3), ma1);
315 final var ma2 = new Matrix(3, 3);
316 calibrator.getInitialMa(ma2);
317 assertEquals(ma1, ma2);
318 assertSame(measurements, calibrator.getMeasurements());
319 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
320 calibrator.getMeasurementType());
321 assertFalse(calibrator.isOrderedMeasurementsRequired());
322 assertFalse(calibrator.isQualityScoresRequired());
323 assertFalse(calibrator.isCommonAxisUsed());
324 assertNull(calibrator.getListener());
325 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
326 assertFalse(calibrator.isReady());
327 assertFalse(calibrator.isRunning());
328 assertNull(calibrator.getEstimatedMa());
329 assertNull(calibrator.getEstimatedSx());
330 assertNull(calibrator.getEstimatedSy());
331 assertNull(calibrator.getEstimatedSz());
332 assertNull(calibrator.getEstimatedMxy());
333 assertNull(calibrator.getEstimatedMxz());
334 assertNull(calibrator.getEstimatedMyx());
335 assertNull(calibrator.getEstimatedMyz());
336 assertNull(calibrator.getEstimatedMzx());
337 assertNull(calibrator.getEstimatedMzy());
338 assertNull(calibrator.getEstimatedCovariance());
339 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
340 assertNull(calibrator.getGroundTruthGravityNorm());
341 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
342 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
343 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
344 }
345
346 @Test
347 void testConstructor4() throws WrongSizeException {
348 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
349 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, this);
350
351
352 assertEquals(0.0, calibrator.getBiasX(), 0.0);
353 assertEquals(0.0, calibrator.getBiasY(), 0.0);
354 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
355 final var bx1 = calibrator.getBiasXAsAcceleration();
356 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
357 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
358 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
359 calibrator.getBiasXAsAcceleration(bx2);
360 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
361 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
362 final var by1 = calibrator.getBiasYAsAcceleration();
363 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
364 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
365 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
366 calibrator.getBiasYAsAcceleration(by2);
367 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
368 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
369 final var bz1 = calibrator.getBiasZAsAcceleration();
370 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
371 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
372 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
373 calibrator.getBiasZAsAcceleration(bz2);
374 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
375 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
376 final var biasTriad1 = calibrator.getBiasAsTriad();
377 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
378 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
379 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
380 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
381 final var biasTriad2 = new AccelerationTriad();
382 calibrator.getBiasAsTriad(biasTriad2);
383 assertEquals(biasTriad1, biasTriad2);
384 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
385 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
386 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
387 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
388 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
389 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
390 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
391 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
392 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
393 final var bias1 = calibrator.getBias();
394 assertArrayEquals(new double[3], bias1, 0.0);
395 final var bias2 = new double[3];
396 calibrator.getBias(bias2);
397 assertArrayEquals(bias1, bias2, 0.0);
398 final var b1 = calibrator.getBiasAsMatrix();
399 assertEquals(new Matrix(3, 1), b1);
400 final var b2 = new Matrix(3, 1);
401 calibrator.getBiasAsMatrix(b2);
402 assertEquals(b1, b2);
403 final var ma1 = calibrator.getInitialMa();
404 assertEquals(new Matrix(3, 3), ma1);
405 final var ma2 = new Matrix(3, 3);
406 calibrator.getInitialMa(ma2);
407 assertEquals(ma1, ma2);
408 assertSame(measurements, calibrator.getMeasurements());
409 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
410 calibrator.getMeasurementType());
411 assertFalse(calibrator.isOrderedMeasurementsRequired());
412 assertFalse(calibrator.isQualityScoresRequired());
413 assertFalse(calibrator.isCommonAxisUsed());
414 assertSame(this, calibrator.getListener());
415 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
416 assertFalse(calibrator.isReady());
417 assertFalse(calibrator.isRunning());
418 assertNull(calibrator.getEstimatedMa());
419 assertNull(calibrator.getEstimatedSx());
420 assertNull(calibrator.getEstimatedSy());
421 assertNull(calibrator.getEstimatedSz());
422 assertNull(calibrator.getEstimatedMxy());
423 assertNull(calibrator.getEstimatedMxz());
424 assertNull(calibrator.getEstimatedMyx());
425 assertNull(calibrator.getEstimatedMyz());
426 assertNull(calibrator.getEstimatedMzx());
427 assertNull(calibrator.getEstimatedMzy());
428 assertNull(calibrator.getEstimatedCovariance());
429 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
430 assertNull(calibrator.getGroundTruthGravityNorm());
431 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
432 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
433 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
434 }
435
436 @Test
437 void testConstructor5() throws WrongSizeException {
438 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true);
439
440
441 assertEquals(0.0, calibrator.getBiasX(), 0.0);
442 assertEquals(0.0, calibrator.getBiasY(), 0.0);
443 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
444 final var bx1 = calibrator.getBiasXAsAcceleration();
445 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
446 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
447 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
448 calibrator.getBiasXAsAcceleration(bx2);
449 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
450 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
451 final var by1 = calibrator.getBiasYAsAcceleration();
452 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
453 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
454 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
455 calibrator.getBiasYAsAcceleration(by2);
456 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
457 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
458 final var bz1 = calibrator.getBiasZAsAcceleration();
459 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
460 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
461 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
462 calibrator.getBiasZAsAcceleration(bz2);
463 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
464 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
465 final var biasTriad1 = calibrator.getBiasAsTriad();
466 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
467 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
468 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
469 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
470 final var biasTriad2 = new AccelerationTriad();
471 calibrator.getBiasAsTriad(biasTriad2);
472 assertEquals(biasTriad1, biasTriad2);
473 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
474 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
475 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
476 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
477 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
478 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
479 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
480 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
481 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
482 final var bias1 = calibrator.getBias();
483 assertArrayEquals(new double[3], bias1, 0.0);
484 final var bias2 = new double[3];
485 calibrator.getBias(bias2);
486 assertArrayEquals(bias1, bias2, 0.0);
487 final var b1 = calibrator.getBiasAsMatrix();
488 assertEquals(new Matrix(3, 1), b1);
489 final var b2 = new Matrix(3, 1);
490 calibrator.getBiasAsMatrix(b2);
491 assertEquals(b1, b2);
492 final var ma1 = calibrator.getInitialMa();
493 assertEquals(new Matrix(3, 3), ma1);
494 final var ma2 = new Matrix(3, 3);
495 calibrator.getInitialMa(ma2);
496 assertEquals(ma1, ma2);
497 assertNull(calibrator.getMeasurements());
498 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
499 calibrator.getMeasurementType());
500 assertFalse(calibrator.isOrderedMeasurementsRequired());
501 assertFalse(calibrator.isQualityScoresRequired());
502 assertTrue(calibrator.isCommonAxisUsed());
503 assertNull(calibrator.getListener());
504 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
505 assertFalse(calibrator.isReady());
506 assertFalse(calibrator.isRunning());
507 assertNull(calibrator.getEstimatedMa());
508 assertNull(calibrator.getEstimatedSx());
509 assertNull(calibrator.getEstimatedSy());
510 assertNull(calibrator.getEstimatedSz());
511 assertNull(calibrator.getEstimatedMxy());
512 assertNull(calibrator.getEstimatedMxz());
513 assertNull(calibrator.getEstimatedMyx());
514 assertNull(calibrator.getEstimatedMyz());
515 assertNull(calibrator.getEstimatedMzx());
516 assertNull(calibrator.getEstimatedMzy());
517 assertNull(calibrator.getEstimatedCovariance());
518 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
519 assertNull(calibrator.getGroundTruthGravityNorm());
520 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
521 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
522 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
523 }
524
525 @Test
526 void testConstructor6() throws WrongSizeException {
527 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, this);
528
529
530 assertEquals(0.0, calibrator.getBiasX(), 0.0);
531 assertEquals(0.0, calibrator.getBiasY(), 0.0);
532 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
533 final var bx1 = calibrator.getBiasXAsAcceleration();
534 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
535 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
536 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
537 calibrator.getBiasXAsAcceleration(bx2);
538 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
539 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
540 final var by1 = calibrator.getBiasYAsAcceleration();
541 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
542 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
543 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
544 calibrator.getBiasYAsAcceleration(by2);
545 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
546 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
547 final var bz1 = calibrator.getBiasZAsAcceleration();
548 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
549 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
550 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
551 calibrator.getBiasZAsAcceleration(bz2);
552 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
553 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
554 final var biasTriad1 = calibrator.getBiasAsTriad();
555 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
556 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
557 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
558 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
559 final var biasTriad2 = new AccelerationTriad();
560 calibrator.getBiasAsTriad(biasTriad2);
561 assertEquals(biasTriad1, biasTriad2);
562 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
563 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
564 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
565 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
566 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
567 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
568 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
569 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
570 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
571 final var bias1 = calibrator.getBias();
572 assertArrayEquals(new double[3], bias1, 0.0);
573 final var bias2 = new double[3];
574 calibrator.getBias(bias2);
575 assertArrayEquals(bias1, bias2, 0.0);
576 final var b1 = calibrator.getBiasAsMatrix();
577 assertEquals(new Matrix(3, 1), b1);
578 final var b2 = new Matrix(3, 1);
579 calibrator.getBiasAsMatrix(b2);
580 assertEquals(b1, b2);
581 final var ma1 = calibrator.getInitialMa();
582 assertEquals(new Matrix(3, 3), ma1);
583 final var ma2 = new Matrix(3, 3);
584 calibrator.getInitialMa(ma2);
585 assertEquals(ma1, ma2);
586 assertNull(calibrator.getMeasurements());
587 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
588 calibrator.getMeasurementType());
589 assertFalse(calibrator.isOrderedMeasurementsRequired());
590 assertFalse(calibrator.isQualityScoresRequired());
591 assertTrue(calibrator.isCommonAxisUsed());
592 assertSame(this, calibrator.getListener());
593 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
594 assertFalse(calibrator.isReady());
595 assertFalse(calibrator.isRunning());
596 assertNull(calibrator.getEstimatedMa());
597 assertNull(calibrator.getEstimatedSx());
598 assertNull(calibrator.getEstimatedSy());
599 assertNull(calibrator.getEstimatedSz());
600 assertNull(calibrator.getEstimatedMxy());
601 assertNull(calibrator.getEstimatedMxz());
602 assertNull(calibrator.getEstimatedMyx());
603 assertNull(calibrator.getEstimatedMyz());
604 assertNull(calibrator.getEstimatedMzx());
605 assertNull(calibrator.getEstimatedMzy());
606 assertNull(calibrator.getEstimatedCovariance());
607 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
608 assertNull(calibrator.getGroundTruthGravityNorm());
609 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
610 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
611 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
612 }
613
614 @Test
615 void testConstructor7() throws WrongSizeException {
616 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
617 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true);
618
619
620 assertEquals(0.0, calibrator.getBiasX(), 0.0);
621 assertEquals(0.0, calibrator.getBiasY(), 0.0);
622 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
623 final var bx1 = calibrator.getBiasXAsAcceleration();
624 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
625 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
626 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
627 calibrator.getBiasXAsAcceleration(bx2);
628 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
629 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
630 final var by1 = calibrator.getBiasYAsAcceleration();
631 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
632 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
633 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
634 calibrator.getBiasYAsAcceleration(by2);
635 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
636 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
637 final var bz1 = calibrator.getBiasZAsAcceleration();
638 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
639 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
640 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
641 calibrator.getBiasZAsAcceleration(bz2);
642 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
643 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
644 final var biasTriad1 = calibrator.getBiasAsTriad();
645 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
646 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
647 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
648 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
649 final var biasTriad2 = new AccelerationTriad();
650 calibrator.getBiasAsTriad(biasTriad2);
651 assertEquals(biasTriad1, biasTriad2);
652 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
653 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
654 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
655 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
656 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
657 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
658 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
659 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
660 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
661 final var bias1 = calibrator.getBias();
662 assertArrayEquals(new double[3], bias1, 0.0);
663 final var bias2 = new double[3];
664 calibrator.getBias(bias2);
665 assertArrayEquals(bias1, bias2, 0.0);
666 final var b1 = calibrator.getBiasAsMatrix();
667 assertEquals(new Matrix(3, 1), b1);
668 final var b2 = new Matrix(3, 1);
669 calibrator.getBiasAsMatrix(b2);
670 assertEquals(b1, b2);
671 final var ma1 = calibrator.getInitialMa();
672 assertEquals(new Matrix(3, 3), ma1);
673 final var ma2 = new Matrix(3, 3);
674 calibrator.getInitialMa(ma2);
675 assertEquals(ma1, ma2);
676 assertSame(measurements, calibrator.getMeasurements());
677 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
678 calibrator.getMeasurementType());
679 assertFalse(calibrator.isOrderedMeasurementsRequired());
680 assertFalse(calibrator.isQualityScoresRequired());
681 assertTrue(calibrator.isCommonAxisUsed());
682 assertNull(calibrator.getListener());
683 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
684 assertFalse(calibrator.isReady());
685 assertFalse(calibrator.isRunning());
686 assertNull(calibrator.getEstimatedMa());
687 assertNull(calibrator.getEstimatedSx());
688 assertNull(calibrator.getEstimatedSy());
689 assertNull(calibrator.getEstimatedSz());
690 assertNull(calibrator.getEstimatedMxy());
691 assertNull(calibrator.getEstimatedMxz());
692 assertNull(calibrator.getEstimatedMyx());
693 assertNull(calibrator.getEstimatedMyz());
694 assertNull(calibrator.getEstimatedMzx());
695 assertNull(calibrator.getEstimatedMzy());
696 assertNull(calibrator.getEstimatedCovariance());
697 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
698 assertNull(calibrator.getGroundTruthGravityNorm());
699 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
700 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
701 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
702 }
703
704 @Test
705 void testConstructor8() throws WrongSizeException {
706 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
707 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
708 this);
709
710
711 assertEquals(0.0, calibrator.getBiasX(), 0.0);
712 assertEquals(0.0, calibrator.getBiasY(), 0.0);
713 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
714 final var bx1 = calibrator.getBiasXAsAcceleration();
715 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
716 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
717 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
718 calibrator.getBiasXAsAcceleration(bx2);
719 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
720 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
721 final var by1 = calibrator.getBiasYAsAcceleration();
722 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
723 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
724 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
725 calibrator.getBiasYAsAcceleration(by2);
726 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
727 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
728 final var bz1 = calibrator.getBiasZAsAcceleration();
729 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
730 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
731 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
732 calibrator.getBiasZAsAcceleration(bz2);
733 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
734 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
735 final var biasTriad1 = calibrator.getBiasAsTriad();
736 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
737 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
738 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
739 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
740 final var biasTriad2 = new AccelerationTriad();
741 calibrator.getBiasAsTriad(biasTriad2);
742 assertEquals(biasTriad1, biasTriad2);
743 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
744 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
745 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
746 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
747 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
748 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
749 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
750 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
751 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
752 final var bias1 = calibrator.getBias();
753 assertArrayEquals(new double[3], bias1, 0.0);
754 final var bias2 = new double[3];
755 calibrator.getBias(bias2);
756 assertArrayEquals(bias1, bias2, 0.0);
757 final var b1 = calibrator.getBiasAsMatrix();
758 assertEquals(new Matrix(3, 1), b1);
759 final var b2 = new Matrix(3, 1);
760 calibrator.getBiasAsMatrix(b2);
761 assertEquals(b1, b2);
762 final var ma1 = calibrator.getInitialMa();
763 assertEquals(new Matrix(3, 3), ma1);
764 final var ma2 = new Matrix(3, 3);
765 calibrator.getInitialMa(ma2);
766 assertEquals(ma1, ma2);
767 assertSame(measurements, calibrator.getMeasurements());
768 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
769 calibrator.getMeasurementType());
770 assertFalse(calibrator.isOrderedMeasurementsRequired());
771 assertFalse(calibrator.isQualityScoresRequired());
772 assertTrue(calibrator.isCommonAxisUsed());
773 assertSame(this, calibrator.getListener());
774 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
775 assertFalse(calibrator.isReady());
776 assertFalse(calibrator.isRunning());
777 assertNull(calibrator.getEstimatedMa());
778 assertNull(calibrator.getEstimatedSx());
779 assertNull(calibrator.getEstimatedSy());
780 assertNull(calibrator.getEstimatedSz());
781 assertNull(calibrator.getEstimatedMxy());
782 assertNull(calibrator.getEstimatedMxz());
783 assertNull(calibrator.getEstimatedMyx());
784 assertNull(calibrator.getEstimatedMyz());
785 assertNull(calibrator.getEstimatedMzx());
786 assertNull(calibrator.getEstimatedMzy());
787 assertNull(calibrator.getEstimatedCovariance());
788 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
789 assertNull(calibrator.getGroundTruthGravityNorm());
790 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
791 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
792 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
793 }
794
795 @Test
796 void testConstructor9() throws WrongSizeException {
797 final var ba = generateBa();
798 final var biasX = ba.getElementAtIndex(0);
799 final var biasY = ba.getElementAtIndex(1);
800 final var biasZ = ba.getElementAtIndex(2);
801
802 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(biasX, biasY, biasZ);
803
804
805 assertEquals(biasX, calibrator.getBiasX(), 0.0);
806 assertEquals(biasY, calibrator.getBiasY(), 0.0);
807 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
808 final var bx1 = calibrator.getBiasXAsAcceleration();
809 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
810 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
811 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
812 calibrator.getBiasXAsAcceleration(bx2);
813 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
814 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
815 final var by1 = calibrator.getBiasYAsAcceleration();
816 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
817 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
818 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
819 calibrator.getBiasYAsAcceleration(by2);
820 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
821 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
822 final var bz1 = calibrator.getBiasZAsAcceleration();
823 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
824 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
825 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
826 calibrator.getBiasZAsAcceleration(bz2);
827 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
828 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
829 final var biasTriad1 = calibrator.getBiasAsTriad();
830 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
831 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
832 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
833 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
834 final var biasTriad2 = new AccelerationTriad();
835 calibrator.getBiasAsTriad(biasTriad2);
836 assertEquals(biasTriad1, biasTriad2);
837 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
838 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
839 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
840 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
841 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
842 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
843 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
844 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
845 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
846 final var bias1 = calibrator.getBias();
847 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
848 final var bias2 = new double[3];
849 calibrator.getBias(bias2);
850 assertArrayEquals(bias1, bias2, 0.0);
851 final var b1 = calibrator.getBiasAsMatrix();
852 assertEquals(b1, ba);
853 final var b2 = new Matrix(3, 1);
854 calibrator.getBiasAsMatrix(b2);
855 assertEquals(b1, b2);
856 final var ma1 = calibrator.getInitialMa();
857 assertEquals(new Matrix(3, 3), ma1);
858 final var ma2 = new Matrix(3, 3);
859 calibrator.getInitialMa(ma2);
860 assertEquals(ma1, ma2);
861 assertNull(calibrator.getMeasurements());
862 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
863 calibrator.getMeasurementType());
864 assertFalse(calibrator.isOrderedMeasurementsRequired());
865 assertFalse(calibrator.isQualityScoresRequired());
866 assertFalse(calibrator.isCommonAxisUsed());
867 assertNull(calibrator.getListener());
868 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
869 assertFalse(calibrator.isReady());
870 assertFalse(calibrator.isRunning());
871 assertNull(calibrator.getEstimatedMa());
872 assertNull(calibrator.getEstimatedSx());
873 assertNull(calibrator.getEstimatedSy());
874 assertNull(calibrator.getEstimatedSz());
875 assertNull(calibrator.getEstimatedMxy());
876 assertNull(calibrator.getEstimatedMxz());
877 assertNull(calibrator.getEstimatedMyx());
878 assertNull(calibrator.getEstimatedMyz());
879 assertNull(calibrator.getEstimatedMzx());
880 assertNull(calibrator.getEstimatedMzy());
881 assertNull(calibrator.getEstimatedCovariance());
882 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
883 assertNull(calibrator.getGroundTruthGravityNorm());
884 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
885 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
886 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
887 }
888
889 @Test
890 void testConstructor10() throws WrongSizeException {
891 final var ba = generateBa();
892 final var biasX = ba.getElementAtIndex(0);
893 final var biasY = ba.getElementAtIndex(1);
894 final var biasZ = ba.getElementAtIndex(2);
895
896 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(biasX, biasY, biasZ, this);
897
898
899 assertEquals(biasX, calibrator.getBiasX(), 0.0);
900 assertEquals(biasY, calibrator.getBiasY(), 0.0);
901 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
902 final var bx1 = calibrator.getBiasXAsAcceleration();
903 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
904 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
905 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
906 calibrator.getBiasXAsAcceleration(bx2);
907 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
908 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
909 final var by1 = calibrator.getBiasYAsAcceleration();
910 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
911 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
912 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
913 calibrator.getBiasYAsAcceleration(by2);
914 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
915 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
916 final var bz1 = calibrator.getBiasZAsAcceleration();
917 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
918 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
919 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
920 calibrator.getBiasZAsAcceleration(bz2);
921 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
922 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
923 final var biasTriad1 = calibrator.getBiasAsTriad();
924 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
925 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
926 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
927 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
928 final var biasTriad2 = new AccelerationTriad();
929 calibrator.getBiasAsTriad(biasTriad2);
930 assertEquals(biasTriad1, biasTriad2);
931 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
932 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
933 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
934 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
935 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
936 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
937 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
938 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
939 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
940 final var bias1 = calibrator.getBias();
941 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
942 final var bias2 = new double[3];
943 calibrator.getBias(bias2);
944 assertArrayEquals(bias1, bias2, 0.0);
945 final var b1 = calibrator.getBiasAsMatrix();
946 assertEquals(b1, ba);
947 final var b2 = new Matrix(3, 1);
948 calibrator.getBiasAsMatrix(b2);
949 assertEquals(b1, b2);
950 final var ma1 = calibrator.getInitialMa();
951 assertEquals(new Matrix(3, 3), ma1);
952 final var ma2 = new Matrix(3, 3);
953 calibrator.getInitialMa(ma2);
954 assertEquals(ma1, ma2);
955 assertNull(calibrator.getMeasurements());
956 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
957 calibrator.getMeasurementType());
958 assertFalse(calibrator.isOrderedMeasurementsRequired());
959 assertFalse(calibrator.isQualityScoresRequired());
960 assertFalse(calibrator.isCommonAxisUsed());
961 assertSame(this, calibrator.getListener());
962 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
963 assertFalse(calibrator.isReady());
964 assertFalse(calibrator.isRunning());
965 assertNull(calibrator.getEstimatedMa());
966 assertNull(calibrator.getEstimatedSx());
967 assertNull(calibrator.getEstimatedSy());
968 assertNull(calibrator.getEstimatedSz());
969 assertNull(calibrator.getEstimatedMxy());
970 assertNull(calibrator.getEstimatedMxz());
971 assertNull(calibrator.getEstimatedMyx());
972 assertNull(calibrator.getEstimatedMyz());
973 assertNull(calibrator.getEstimatedMzx());
974 assertNull(calibrator.getEstimatedMzy());
975 assertNull(calibrator.getEstimatedCovariance());
976 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
977 assertNull(calibrator.getGroundTruthGravityNorm());
978 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
979 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
980 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
981 }
982
983 @Test
984 void testConstructor11() throws WrongSizeException {
985 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
986
987 final var ba = generateBa();
988 final var biasX = ba.getElementAtIndex(0);
989 final var biasY = ba.getElementAtIndex(1);
990 final var biasZ = ba.getElementAtIndex(2);
991
992 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, biasX, biasY, biasZ);
993
994
995 assertEquals(biasX, calibrator.getBiasX(), 0.0);
996 assertEquals(biasY, calibrator.getBiasY(), 0.0);
997 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
998 final var bx1 = calibrator.getBiasXAsAcceleration();
999 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1000 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1001 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1002 calibrator.getBiasXAsAcceleration(bx2);
1003 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1004 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1005 final var by1 = calibrator.getBiasYAsAcceleration();
1006 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1007 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1008 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1009 calibrator.getBiasYAsAcceleration(by2);
1010 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1011 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1012 final var bz1 = calibrator.getBiasZAsAcceleration();
1013 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1014 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1015 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1016 calibrator.getBiasZAsAcceleration(bz2);
1017 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1018 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1019 final var biasTriad1 = calibrator.getBiasAsTriad();
1020 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1021 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1022 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1023 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1024 final var biasTriad2 = new AccelerationTriad();
1025 calibrator.getBiasAsTriad(biasTriad2);
1026 assertEquals(biasTriad1, biasTriad2);
1027 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1028 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1029 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1030 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1031 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1032 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1033 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1034 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1035 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1036 final var bias1 = calibrator.getBias();
1037 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1038 final var bias2 = new double[3];
1039 calibrator.getBias(bias2);
1040 assertArrayEquals(bias1, bias2, 0.0);
1041 final var b1 = calibrator.getBiasAsMatrix();
1042 assertEquals(b1, ba);
1043 final var b2 = new Matrix(3, 1);
1044 calibrator.getBiasAsMatrix(b2);
1045 assertEquals(b1, b2);
1046 final var ma1 = calibrator.getInitialMa();
1047 assertEquals(new Matrix(3, 3), ma1);
1048 final var ma2 = new Matrix(3, 3);
1049 calibrator.getInitialMa(ma2);
1050 assertEquals(ma1, ma2);
1051 assertSame(measurements, calibrator.getMeasurements());
1052 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1053 calibrator.getMeasurementType());
1054 assertFalse(calibrator.isOrderedMeasurementsRequired());
1055 assertFalse(calibrator.isQualityScoresRequired());
1056 assertFalse(calibrator.isCommonAxisUsed());
1057 assertNull(calibrator.getListener());
1058 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
1059 assertFalse(calibrator.isReady());
1060 assertFalse(calibrator.isRunning());
1061 assertNull(calibrator.getEstimatedMa());
1062 assertNull(calibrator.getEstimatedSx());
1063 assertNull(calibrator.getEstimatedSy());
1064 assertNull(calibrator.getEstimatedSz());
1065 assertNull(calibrator.getEstimatedMxy());
1066 assertNull(calibrator.getEstimatedMxz());
1067 assertNull(calibrator.getEstimatedMyx());
1068 assertNull(calibrator.getEstimatedMyz());
1069 assertNull(calibrator.getEstimatedMzx());
1070 assertNull(calibrator.getEstimatedMzy());
1071 assertNull(calibrator.getEstimatedCovariance());
1072 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1073 assertNull(calibrator.getGroundTruthGravityNorm());
1074 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1075 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1076 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1077 }
1078
1079 @Test
1080 void testConstructor12() throws WrongSizeException {
1081 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
1082
1083 final var ba = generateBa();
1084 final var biasX = ba.getElementAtIndex(0);
1085 final var biasY = ba.getElementAtIndex(1);
1086 final var biasZ = ba.getElementAtIndex(2);
1087
1088 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
1089 this);
1090
1091
1092 assertEquals(biasX, calibrator.getBiasX(), 0.0);
1093 assertEquals(biasY, calibrator.getBiasY(), 0.0);
1094 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1095 final var bx1 = calibrator.getBiasXAsAcceleration();
1096 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1097 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1098 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1099 calibrator.getBiasXAsAcceleration(bx2);
1100 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1101 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1102 final var by1 = calibrator.getBiasYAsAcceleration();
1103 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1104 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1105 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1106 calibrator.getBiasYAsAcceleration(by2);
1107 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1108 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1109 final var bz1 = calibrator.getBiasZAsAcceleration();
1110 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1111 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1112 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1113 calibrator.getBiasZAsAcceleration(bz2);
1114 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1115 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1116 final var biasTriad1 = calibrator.getBiasAsTriad();
1117 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1118 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1119 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1120 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1121 final var biasTriad2 = new AccelerationTriad();
1122 calibrator.getBiasAsTriad(biasTriad2);
1123 assertEquals(biasTriad1, biasTriad2);
1124 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1125 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1126 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1127 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1128 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1129 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1130 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1131 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1132 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1133 final var bias1 = calibrator.getBias();
1134 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1135 final var bias2 = new double[3];
1136 calibrator.getBias(bias2);
1137 assertArrayEquals(bias1, bias2, 0.0);
1138 final var b1 = calibrator.getBiasAsMatrix();
1139 assertEquals(b1, ba);
1140 final var b2 = new Matrix(3, 1);
1141 calibrator.getBiasAsMatrix(b2);
1142 assertEquals(b1, b2);
1143 final var ma1 = calibrator.getInitialMa();
1144 assertEquals(new Matrix(3, 3), ma1);
1145 final var ma2 = new Matrix(3, 3);
1146 calibrator.getInitialMa(ma2);
1147 assertEquals(ma1, ma2);
1148 assertSame(measurements, calibrator.getMeasurements());
1149 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1150 calibrator.getMeasurementType());
1151 assertFalse(calibrator.isOrderedMeasurementsRequired());
1152 assertFalse(calibrator.isQualityScoresRequired());
1153 assertFalse(calibrator.isCommonAxisUsed());
1154 assertSame(this, calibrator.getListener());
1155 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
1156 assertFalse(calibrator.isReady());
1157 assertFalse(calibrator.isRunning());
1158 assertNull(calibrator.getEstimatedMa());
1159 assertNull(calibrator.getEstimatedSx());
1160 assertNull(calibrator.getEstimatedSy());
1161 assertNull(calibrator.getEstimatedSz());
1162 assertNull(calibrator.getEstimatedMxy());
1163 assertNull(calibrator.getEstimatedMxz());
1164 assertNull(calibrator.getEstimatedMyx());
1165 assertNull(calibrator.getEstimatedMyz());
1166 assertNull(calibrator.getEstimatedMzx());
1167 assertNull(calibrator.getEstimatedMzy());
1168 assertNull(calibrator.getEstimatedCovariance());
1169 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1170 assertNull(calibrator.getGroundTruthGravityNorm());
1171 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1172 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1173 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1174 }
1175
1176 @Test
1177 void testConstructor13() throws WrongSizeException {
1178 final var ba = generateBa();
1179 final var biasX = ba.getElementAtIndex(0);
1180 final var biasY = ba.getElementAtIndex(1);
1181 final var biasZ = ba.getElementAtIndex(2);
1182
1183 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true,
1184 biasX, biasY, biasZ);
1185
1186
1187 assertEquals(biasX, calibrator.getBiasX(), 0.0);
1188 assertEquals(biasY, calibrator.getBiasY(), 0.0);
1189 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1190 final var bx1 = calibrator.getBiasXAsAcceleration();
1191 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1192 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1193 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1194 calibrator.getBiasXAsAcceleration(bx2);
1195 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1196 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1197 final var by1 = calibrator.getBiasYAsAcceleration();
1198 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1199 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1200 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1201 calibrator.getBiasYAsAcceleration(by2);
1202 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1203 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1204 final var bz1 = calibrator.getBiasZAsAcceleration();
1205 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1206 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1207 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1208 calibrator.getBiasZAsAcceleration(bz2);
1209 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1210 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1211 final var biasTriad1 = calibrator.getBiasAsTriad();
1212 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1213 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1214 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1215 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1216 final var biasTriad2 = new AccelerationTriad();
1217 calibrator.getBiasAsTriad(biasTriad2);
1218 assertEquals(biasTriad1, biasTriad2);
1219 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1220 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1221 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1222 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1223 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1224 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1225 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1226 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1227 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1228 final var bias1 = calibrator.getBias();
1229 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1230 final var bias2 = new double[3];
1231 calibrator.getBias(bias2);
1232 assertArrayEquals(bias1, bias2, 0.0);
1233 final var b1 = calibrator.getBiasAsMatrix();
1234 assertEquals(b1, ba);
1235 final var b2 = new Matrix(3, 1);
1236 calibrator.getBiasAsMatrix(b2);
1237 assertEquals(b1, b2);
1238 final var ma1 = calibrator.getInitialMa();
1239 assertEquals(new Matrix(3, 3), ma1);
1240 final var ma2 = new Matrix(3, 3);
1241 calibrator.getInitialMa(ma2);
1242 assertEquals(ma1, ma2);
1243 assertNull(calibrator.getMeasurements());
1244 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1245 calibrator.getMeasurementType());
1246 assertFalse(calibrator.isOrderedMeasurementsRequired());
1247 assertFalse(calibrator.isQualityScoresRequired());
1248 assertTrue(calibrator.isCommonAxisUsed());
1249 assertNull(calibrator.getListener());
1250 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
1251 assertFalse(calibrator.isReady());
1252 assertFalse(calibrator.isRunning());
1253 assertNull(calibrator.getEstimatedMa());
1254 assertNull(calibrator.getEstimatedSx());
1255 assertNull(calibrator.getEstimatedSy());
1256 assertNull(calibrator.getEstimatedSz());
1257 assertNull(calibrator.getEstimatedMxy());
1258 assertNull(calibrator.getEstimatedMxz());
1259 assertNull(calibrator.getEstimatedMyx());
1260 assertNull(calibrator.getEstimatedMyz());
1261 assertNull(calibrator.getEstimatedMzx());
1262 assertNull(calibrator.getEstimatedMzy());
1263 assertNull(calibrator.getEstimatedCovariance());
1264 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1265 assertNull(calibrator.getGroundTruthGravityNorm());
1266 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1267 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1268 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1269 }
1270
1271 @Test
1272 void testConstructor14() throws WrongSizeException {
1273 final var ba = generateBa();
1274 final var biasX = ba.getElementAtIndex(0);
1275 final var biasY = ba.getElementAtIndex(1);
1276 final var biasZ = ba.getElementAtIndex(2);
1277
1278 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true,
1279 biasX, biasY, biasZ, this);
1280
1281
1282 assertEquals(biasX, calibrator.getBiasX(), 0.0);
1283 assertEquals(biasY, calibrator.getBiasY(), 0.0);
1284 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1285 final var bx1 = calibrator.getBiasXAsAcceleration();
1286 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1287 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1288 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1289 calibrator.getBiasXAsAcceleration(bx2);
1290 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1291 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1292 final var by1 = calibrator.getBiasYAsAcceleration();
1293 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1294 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1295 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1296 calibrator.getBiasYAsAcceleration(by2);
1297 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1298 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1299 final var bz1 = calibrator.getBiasZAsAcceleration();
1300 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1301 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1302 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1303 calibrator.getBiasZAsAcceleration(bz2);
1304 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1305 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1306 final var biasTriad1 = calibrator.getBiasAsTriad();
1307 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1308 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1309 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1310 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1311 final var biasTriad2 = new AccelerationTriad();
1312 calibrator.getBiasAsTriad(biasTriad2);
1313 assertEquals(biasTriad1, biasTriad2);
1314 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1315 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1316 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1317 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1318 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1319 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1320 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1321 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1322 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1323 final var bias1 = calibrator.getBias();
1324 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1325 final var bias2 = new double[3];
1326 calibrator.getBias(bias2);
1327 assertArrayEquals(bias1, bias2, 0.0);
1328 final var b1 = calibrator.getBiasAsMatrix();
1329 assertEquals(b1, ba);
1330 final var b2 = new Matrix(3, 1);
1331 calibrator.getBiasAsMatrix(b2);
1332 assertEquals(b1, b2);
1333 final var ma1 = calibrator.getInitialMa();
1334 assertEquals(new Matrix(3, 3), ma1);
1335 final var ma2 = new Matrix(3, 3);
1336 calibrator.getInitialMa(ma2);
1337 assertEquals(ma1, ma2);
1338 assertNull(calibrator.getMeasurements());
1339 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1340 calibrator.getMeasurementType());
1341 assertFalse(calibrator.isOrderedMeasurementsRequired());
1342 assertFalse(calibrator.isQualityScoresRequired());
1343 assertTrue(calibrator.isCommonAxisUsed());
1344 assertSame(this, calibrator.getListener());
1345 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
1346 assertFalse(calibrator.isReady());
1347 assertFalse(calibrator.isRunning());
1348 assertNull(calibrator.getEstimatedMa());
1349 assertNull(calibrator.getEstimatedSx());
1350 assertNull(calibrator.getEstimatedSy());
1351 assertNull(calibrator.getEstimatedSz());
1352 assertNull(calibrator.getEstimatedMxy());
1353 assertNull(calibrator.getEstimatedMxz());
1354 assertNull(calibrator.getEstimatedMyx());
1355 assertNull(calibrator.getEstimatedMyz());
1356 assertNull(calibrator.getEstimatedMzx());
1357 assertNull(calibrator.getEstimatedMzy());
1358 assertNull(calibrator.getEstimatedCovariance());
1359 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1360 assertNull(calibrator.getGroundTruthGravityNorm());
1361 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1362 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1363 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1364 }
1365
1366 @Test
1367 void testConstructor15() throws WrongSizeException {
1368 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
1369
1370 final var ba = generateBa();
1371 final var biasX = ba.getElementAtIndex(0);
1372 final var biasY = ba.getElementAtIndex(1);
1373 final var biasZ = ba.getElementAtIndex(2);
1374
1375 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
1376 biasX, biasY, biasZ);
1377
1378
1379 assertEquals(biasX, calibrator.getBiasX(), 0.0);
1380 assertEquals(biasY, calibrator.getBiasY(), 0.0);
1381 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1382 final var bx1 = calibrator.getBiasXAsAcceleration();
1383 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1384 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1385 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1386 calibrator.getBiasXAsAcceleration(bx2);
1387 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1388 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1389 final var by1 = calibrator.getBiasYAsAcceleration();
1390 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1391 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1392 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1393 calibrator.getBiasYAsAcceleration(by2);
1394 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1395 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1396 final var bz1 = calibrator.getBiasZAsAcceleration();
1397 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1398 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1399 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1400 calibrator.getBiasZAsAcceleration(bz2);
1401 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1402 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1403 final var biasTriad1 = calibrator.getBiasAsTriad();
1404 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1405 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1406 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1407 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1408 final var biasTriad2 = new AccelerationTriad();
1409 calibrator.getBiasAsTriad(biasTriad2);
1410 assertEquals(biasTriad1, biasTriad2);
1411 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1412 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1413 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1414 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1415 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1416 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1417 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1418 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1419 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1420 final var bias1 = calibrator.getBias();
1421 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1422 final var bias2 = new double[3];
1423 calibrator.getBias(bias2);
1424 assertArrayEquals(bias1, bias2, 0.0);
1425 final var b1 = calibrator.getBiasAsMatrix();
1426 assertEquals(b1, ba);
1427 final var b2 = new Matrix(3, 1);
1428 calibrator.getBiasAsMatrix(b2);
1429 assertEquals(b1, b2);
1430 final var ma1 = calibrator.getInitialMa();
1431 assertEquals(new Matrix(3, 3), ma1);
1432 final var ma2 = new Matrix(3, 3);
1433 calibrator.getInitialMa(ma2);
1434 assertEquals(ma1, ma2);
1435 assertSame(measurements, calibrator.getMeasurements());
1436 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1437 calibrator.getMeasurementType());
1438 assertFalse(calibrator.isOrderedMeasurementsRequired());
1439 assertFalse(calibrator.isQualityScoresRequired());
1440 assertTrue(calibrator.isCommonAxisUsed());
1441 assertNull(calibrator.getListener());
1442 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
1443 assertFalse(calibrator.isReady());
1444 assertFalse(calibrator.isRunning());
1445 assertNull(calibrator.getEstimatedMa());
1446 assertNull(calibrator.getEstimatedSx());
1447 assertNull(calibrator.getEstimatedSy());
1448 assertNull(calibrator.getEstimatedSz());
1449 assertNull(calibrator.getEstimatedMxy());
1450 assertNull(calibrator.getEstimatedMxz());
1451 assertNull(calibrator.getEstimatedMyx());
1452 assertNull(calibrator.getEstimatedMyz());
1453 assertNull(calibrator.getEstimatedMzx());
1454 assertNull(calibrator.getEstimatedMzy());
1455 assertNull(calibrator.getEstimatedCovariance());
1456 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1457 assertNull(calibrator.getGroundTruthGravityNorm());
1458 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1459 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1460 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1461 }
1462
1463 @Test
1464 void testConstructor16() throws WrongSizeException {
1465 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
1466
1467 final var ba = generateBa();
1468 final var biasX = ba.getElementAtIndex(0);
1469 final var biasY = ba.getElementAtIndex(1);
1470 final var biasZ = ba.getElementAtIndex(2);
1471
1472 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
1473 biasX, biasY, biasZ, this);
1474
1475
1476 assertEquals(biasX, calibrator.getBiasX(), 0.0);
1477 assertEquals(biasY, calibrator.getBiasY(), 0.0);
1478 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1479 final var bx1 = calibrator.getBiasXAsAcceleration();
1480 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1481 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1482 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1483 calibrator.getBiasXAsAcceleration(bx2);
1484 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1485 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1486 final var by1 = calibrator.getBiasYAsAcceleration();
1487 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1488 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1489 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1490 calibrator.getBiasYAsAcceleration(by2);
1491 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1492 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1493 final var bz1 = calibrator.getBiasZAsAcceleration();
1494 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1495 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1496 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1497 calibrator.getBiasZAsAcceleration(bz2);
1498 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1499 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1500 final var biasTriad1 = calibrator.getBiasAsTriad();
1501 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1502 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1503 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1504 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1505 final var biasTriad2 = new AccelerationTriad();
1506 calibrator.getBiasAsTriad(biasTriad2);
1507 assertEquals(biasTriad1, biasTriad2);
1508 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1509 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1510 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1511 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1512 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1513 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1514 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1515 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1516 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1517 final var bias1 = calibrator.getBias();
1518 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1519 final var bias2 = new double[3];
1520 calibrator.getBias(bias2);
1521 assertArrayEquals(bias1, bias2, 0.0);
1522 final var b1 = calibrator.getBiasAsMatrix();
1523 assertEquals(b1, ba);
1524 final var b2 = new Matrix(3, 1);
1525 calibrator.getBiasAsMatrix(b2);
1526 assertEquals(b1, b2);
1527 final var ma1 = calibrator.getInitialMa();
1528 assertEquals(new Matrix(3, 3), ma1);
1529 final var ma2 = new Matrix(3, 3);
1530 calibrator.getInitialMa(ma2);
1531 assertEquals(ma1, ma2);
1532 assertSame(calibrator.getMeasurements(), measurements);
1533 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1534 calibrator.getMeasurementType());
1535 assertFalse(calibrator.isOrderedMeasurementsRequired());
1536 assertFalse(calibrator.isQualityScoresRequired());
1537 assertTrue(calibrator.isCommonAxisUsed());
1538 assertSame(this, calibrator.getListener());
1539 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
1540 assertFalse(calibrator.isReady());
1541 assertFalse(calibrator.isRunning());
1542 assertNull(calibrator.getEstimatedMa());
1543 assertNull(calibrator.getEstimatedSx());
1544 assertNull(calibrator.getEstimatedSy());
1545 assertNull(calibrator.getEstimatedSz());
1546 assertNull(calibrator.getEstimatedMxy());
1547 assertNull(calibrator.getEstimatedMxz());
1548 assertNull(calibrator.getEstimatedMyx());
1549 assertNull(calibrator.getEstimatedMyz());
1550 assertNull(calibrator.getEstimatedMzx());
1551 assertNull(calibrator.getEstimatedMzy());
1552 assertNull(calibrator.getEstimatedCovariance());
1553 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1554 assertNull(calibrator.getGroundTruthGravityNorm());
1555 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1556 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1557 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1558 }
1559
1560 @Test
1561 void testConstructor17() throws WrongSizeException {
1562 final var ba = generateBa();
1563 final var biasX = ba.getElementAtIndex(0);
1564 final var biasY = ba.getElementAtIndex(1);
1565 final var biasZ = ba.getElementAtIndex(2);
1566
1567 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1568 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1569 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1570
1571 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz);
1572
1573
1574 assertEquals(biasX, calibrator.getBiasX(), 0.0);
1575 assertEquals(biasY, calibrator.getBiasY(), 0.0);
1576 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1577 final var bx1 = calibrator.getBiasXAsAcceleration();
1578 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1579 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1580 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1581 calibrator.getBiasXAsAcceleration(bx2);
1582 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1583 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1584 final var by1 = calibrator.getBiasYAsAcceleration();
1585 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1586 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1587 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1588 calibrator.getBiasYAsAcceleration(by2);
1589 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1590 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1591 final var bz1 = calibrator.getBiasZAsAcceleration();
1592 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1593 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1594 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1595 calibrator.getBiasZAsAcceleration(bz2);
1596 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1597 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1598 final var biasTriad1 = calibrator.getBiasAsTriad();
1599 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1600 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1601 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1602 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1603 final var biasTriad2 = new AccelerationTriad();
1604 calibrator.getBiasAsTriad(biasTriad2);
1605 assertEquals(biasTriad1, biasTriad2);
1606 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1607 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1608 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1609 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1610 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1611 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1612 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1613 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1614 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1615 final var bias1 = calibrator.getBias();
1616 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1617 final var bias2 = new double[3];
1618 calibrator.getBias(bias2);
1619 assertArrayEquals(bias1, bias2, 0.0);
1620 final var b1 = calibrator.getBiasAsMatrix();
1621 assertEquals(b1, ba);
1622 final var b2 = new Matrix(3, 1);
1623 calibrator.getBiasAsMatrix(b2);
1624 assertEquals(b1, b2);
1625 final var ma1 = calibrator.getInitialMa();
1626 assertEquals(new Matrix(3, 3), ma1);
1627 final var ma2 = new Matrix(3, 3);
1628 calibrator.getInitialMa(ma2);
1629 assertEquals(ma1, ma2);
1630 assertNull(calibrator.getMeasurements());
1631 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1632 calibrator.getMeasurementType());
1633 assertFalse(calibrator.isOrderedMeasurementsRequired());
1634 assertFalse(calibrator.isQualityScoresRequired());
1635 assertFalse(calibrator.isCommonAxisUsed());
1636 assertNull(calibrator.getListener());
1637 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
1638 assertFalse(calibrator.isReady());
1639 assertFalse(calibrator.isRunning());
1640 assertNull(calibrator.getEstimatedMa());
1641 assertNull(calibrator.getEstimatedSx());
1642 assertNull(calibrator.getEstimatedSy());
1643 assertNull(calibrator.getEstimatedSz());
1644 assertNull(calibrator.getEstimatedMxy());
1645 assertNull(calibrator.getEstimatedMxz());
1646 assertNull(calibrator.getEstimatedMyx());
1647 assertNull(calibrator.getEstimatedMyz());
1648 assertNull(calibrator.getEstimatedMzx());
1649 assertNull(calibrator.getEstimatedMzy());
1650 assertNull(calibrator.getEstimatedCovariance());
1651 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1652 assertNull(calibrator.getGroundTruthGravityNorm());
1653 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1654 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1655 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1656 }
1657
1658 @Test
1659 void testConstructor18() throws WrongSizeException {
1660 final var ba = generateBa();
1661 final var biasX = ba.getElementAtIndex(0);
1662 final var biasY = ba.getElementAtIndex(1);
1663 final var biasZ = ba.getElementAtIndex(2);
1664
1665 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1666 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1667 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1668
1669 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz, this);
1670
1671
1672 assertEquals(calibrator.getBiasX(), biasX, 0.0);
1673 assertEquals(calibrator.getBiasY(), biasY, 0.0);
1674 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1675 final var bx1 = calibrator.getBiasXAsAcceleration();
1676 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1677 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1678 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1679 calibrator.getBiasXAsAcceleration(bx2);
1680 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1681 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1682 final var by1 = calibrator.getBiasYAsAcceleration();
1683 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1684 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1685 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1686 calibrator.getBiasYAsAcceleration(by2);
1687 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1688 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1689 final var bz1 = calibrator.getBiasZAsAcceleration();
1690 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1691 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1692 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1693 calibrator.getBiasZAsAcceleration(bz2);
1694 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1695 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1696 final var biasTriad1 = calibrator.getBiasAsTriad();
1697 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1698 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1699 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1700 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1701 final var biasTriad2 = new AccelerationTriad();
1702 calibrator.getBiasAsTriad(biasTriad2);
1703 assertEquals(biasTriad1, biasTriad2);
1704 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1705 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1706 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1707 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1708 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1709 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1710 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1711 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1712 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1713 final var bias1 = calibrator.getBias();
1714 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1715 final var bias2 = new double[3];
1716 calibrator.getBias(bias2);
1717 assertArrayEquals(bias1, bias2, 0.0);
1718 final var b1 = calibrator.getBiasAsMatrix();
1719 assertEquals(b1, ba);
1720 final var b2 = new Matrix(3, 1);
1721 calibrator.getBiasAsMatrix(b2);
1722 assertEquals(b1, b2);
1723 final var ma1 = calibrator.getInitialMa();
1724 assertEquals(new Matrix(3, 3), ma1);
1725 final var ma2 = new Matrix(3, 3);
1726 calibrator.getInitialMa(ma2);
1727 assertEquals(ma1, ma2);
1728 assertNull(calibrator.getMeasurements());
1729 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1730 calibrator.getMeasurementType());
1731 assertFalse(calibrator.isOrderedMeasurementsRequired());
1732 assertFalse(calibrator.isQualityScoresRequired());
1733 assertFalse(calibrator.isCommonAxisUsed());
1734 assertSame(this, calibrator.getListener());
1735 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
1736 assertFalse(calibrator.isReady());
1737 assertFalse(calibrator.isRunning());
1738 assertNull(calibrator.getEstimatedMa());
1739 assertNull(calibrator.getEstimatedSx());
1740 assertNull(calibrator.getEstimatedSy());
1741 assertNull(calibrator.getEstimatedSz());
1742 assertNull(calibrator.getEstimatedMxy());
1743 assertNull(calibrator.getEstimatedMxz());
1744 assertNull(calibrator.getEstimatedMyx());
1745 assertNull(calibrator.getEstimatedMyz());
1746 assertNull(calibrator.getEstimatedMzx());
1747 assertNull(calibrator.getEstimatedMzy());
1748 assertNull(calibrator.getEstimatedCovariance());
1749 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1750 assertNull(calibrator.getGroundTruthGravityNorm());
1751 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1752 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1753 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1754 }
1755
1756 @Test
1757 void testConstructor19() throws WrongSizeException {
1758 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
1759
1760 final var ba = generateBa();
1761 final var biasX = ba.getElementAtIndex(0);
1762 final var biasY = ba.getElementAtIndex(1);
1763 final var biasZ = ba.getElementAtIndex(2);
1764
1765 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1766 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1767 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1768
1769 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bx, by, bz);
1770
1771
1772 assertEquals(biasX, calibrator.getBiasX(), 0.0);
1773 assertEquals(biasY, calibrator.getBiasY(), 0.0);
1774 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1775 final var bx1 = calibrator.getBiasXAsAcceleration();
1776 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1777 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1778 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1779 calibrator.getBiasXAsAcceleration(bx2);
1780 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1781 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1782 final var by1 = calibrator.getBiasYAsAcceleration();
1783 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1784 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1785 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1786 calibrator.getBiasYAsAcceleration(by2);
1787 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1788 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1789 final var bz1 = calibrator.getBiasZAsAcceleration();
1790 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1791 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1792 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1793 calibrator.getBiasZAsAcceleration(bz2);
1794 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1795 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1796 final var biasTriad1 = calibrator.getBiasAsTriad();
1797 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1798 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1799 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1800 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1801 final var biasTriad2 = new AccelerationTriad();
1802 calibrator.getBiasAsTriad(biasTriad2);
1803 assertEquals(biasTriad1, biasTriad2);
1804 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1805 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1806 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1807 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1808 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1809 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1810 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1811 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1812 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1813 final var bias1 = calibrator.getBias();
1814 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1815 final var bias2 = new double[3];
1816 calibrator.getBias(bias2);
1817 assertArrayEquals(bias1, bias2, 0.0);
1818 final var b1 = calibrator.getBiasAsMatrix();
1819 assertEquals(b1, ba);
1820 final var b2 = new Matrix(3, 1);
1821 calibrator.getBiasAsMatrix(b2);
1822 assertEquals(b1, b2);
1823 final var ma1 = calibrator.getInitialMa();
1824 assertEquals(new Matrix(3, 3), ma1);
1825 final var ma2 = new Matrix(3, 3);
1826 calibrator.getInitialMa(ma2);
1827 assertEquals(ma1, ma2);
1828 assertSame(calibrator.getMeasurements(), measurements);
1829 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1830 calibrator.getMeasurementType());
1831 assertFalse(calibrator.isOrderedMeasurementsRequired());
1832 assertFalse(calibrator.isQualityScoresRequired());
1833 assertFalse(calibrator.isCommonAxisUsed());
1834 assertNull(calibrator.getListener());
1835 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
1836 assertFalse(calibrator.isReady());
1837 assertFalse(calibrator.isRunning());
1838 assertNull(calibrator.getEstimatedMa());
1839 assertNull(calibrator.getEstimatedSx());
1840 assertNull(calibrator.getEstimatedSy());
1841 assertNull(calibrator.getEstimatedSz());
1842 assertNull(calibrator.getEstimatedMxy());
1843 assertNull(calibrator.getEstimatedMxz());
1844 assertNull(calibrator.getEstimatedMyx());
1845 assertNull(calibrator.getEstimatedMyz());
1846 assertNull(calibrator.getEstimatedMzx());
1847 assertNull(calibrator.getEstimatedMzy());
1848 assertNull(calibrator.getEstimatedCovariance());
1849 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1850 assertNull(calibrator.getGroundTruthGravityNorm());
1851 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1852 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1853 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1854 }
1855
1856 @Test
1857 void testConstructor20() throws WrongSizeException {
1858 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
1859
1860 final var ba = generateBa();
1861 final var biasX = ba.getElementAtIndex(0);
1862 final var biasY = ba.getElementAtIndex(1);
1863 final var biasZ = ba.getElementAtIndex(2);
1864
1865 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1866 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1867 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1868
1869 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bx, by, bz,
1870 this);
1871
1872
1873 assertEquals(biasX, calibrator.getBiasX(), 0.0);
1874 assertEquals(biasY, calibrator.getBiasY(), 0.0);
1875 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1876 final var bx1 = calibrator.getBiasXAsAcceleration();
1877 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1878 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1879 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1880 calibrator.getBiasXAsAcceleration(bx2);
1881 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1882 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1883 final var by1 = calibrator.getBiasYAsAcceleration();
1884 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1885 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1886 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1887 calibrator.getBiasYAsAcceleration(by2);
1888 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1889 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1890 final var bz1 = calibrator.getBiasZAsAcceleration();
1891 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1892 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1893 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1894 calibrator.getBiasZAsAcceleration(bz2);
1895 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1896 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1897 final var biasTriad1 = calibrator.getBiasAsTriad();
1898 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1899 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1900 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1901 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1902 final var biasTriad2 = new AccelerationTriad();
1903 calibrator.getBiasAsTriad(biasTriad2);
1904 assertEquals(biasTriad1, biasTriad2);
1905 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1906 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1907 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1908 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1909 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1910 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1911 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1912 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1913 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1914 final var bias1 = calibrator.getBias();
1915 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1916 final var bias2 = new double[3];
1917 calibrator.getBias(bias2);
1918 assertArrayEquals(bias1, bias2, 0.0);
1919 final var b1 = calibrator.getBiasAsMatrix();
1920 assertEquals(b1, ba);
1921 final var b2 = new Matrix(3, 1);
1922 calibrator.getBiasAsMatrix(b2);
1923 assertEquals(b1, b2);
1924 final var ma1 = calibrator.getInitialMa();
1925 assertEquals(new Matrix(3, 3), ma1);
1926 final var ma2 = new Matrix(3, 3);
1927 calibrator.getInitialMa(ma2);
1928 assertEquals(ma1, ma2);
1929 assertSame(measurements, calibrator.getMeasurements());
1930 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1931 calibrator.getMeasurementType());
1932 assertFalse(calibrator.isOrderedMeasurementsRequired());
1933 assertFalse(calibrator.isQualityScoresRequired());
1934 assertFalse(calibrator.isCommonAxisUsed());
1935 assertSame(this, calibrator.getListener());
1936 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
1937 assertFalse(calibrator.isReady());
1938 assertFalse(calibrator.isRunning());
1939 assertNull(calibrator.getEstimatedMa());
1940 assertNull(calibrator.getEstimatedSx());
1941 assertNull(calibrator.getEstimatedSy());
1942 assertNull(calibrator.getEstimatedSz());
1943 assertNull(calibrator.getEstimatedMxy());
1944 assertNull(calibrator.getEstimatedMxz());
1945 assertNull(calibrator.getEstimatedMyx());
1946 assertNull(calibrator.getEstimatedMyz());
1947 assertNull(calibrator.getEstimatedMzx());
1948 assertNull(calibrator.getEstimatedMzy());
1949 assertNull(calibrator.getEstimatedCovariance());
1950 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1951 assertNull(calibrator.getGroundTruthGravityNorm());
1952 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1953 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1954 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1955 }
1956
1957 @Test
1958 void testConstructor21() throws WrongSizeException {
1959 final var ba = generateBa();
1960 final var biasX = ba.getElementAtIndex(0);
1961 final var biasY = ba.getElementAtIndex(1);
1962 final var biasZ = ba.getElementAtIndex(2);
1963
1964 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1965 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1966 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1967
1968 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bx, by, bz);
1969
1970
1971 assertEquals(biasX, calibrator.getBiasX(), 0.0);
1972 assertEquals(biasY, calibrator.getBiasY(), 0.0);
1973 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1974 final var bx1 = calibrator.getBiasXAsAcceleration();
1975 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1976 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1977 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1978 calibrator.getBiasXAsAcceleration(bx2);
1979 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1980 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1981 final var by1 = calibrator.getBiasYAsAcceleration();
1982 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1983 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1984 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1985 calibrator.getBiasYAsAcceleration(by2);
1986 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1987 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1988 final var bz1 = calibrator.getBiasZAsAcceleration();
1989 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1990 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1991 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1992 calibrator.getBiasZAsAcceleration(bz2);
1993 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1994 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1995 final var biasTriad1 = calibrator.getBiasAsTriad();
1996 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1997 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1998 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1999 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2000 final var biasTriad2 = new AccelerationTriad();
2001 calibrator.getBiasAsTriad(biasTriad2);
2002 assertEquals(biasTriad1, biasTriad2);
2003 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
2004 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
2005 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
2006 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2007 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2008 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2009 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2010 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2011 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2012 final var bias1 = calibrator.getBias();
2013 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2014 final var bias2 = new double[3];
2015 calibrator.getBias(bias2);
2016 assertArrayEquals(bias1, bias2, 0.0);
2017 final var b1 = calibrator.getBiasAsMatrix();
2018 assertEquals(b1, ba);
2019 final var b2 = new Matrix(3, 1);
2020 calibrator.getBiasAsMatrix(b2);
2021 assertEquals(b1, b2);
2022 final var ma1 = calibrator.getInitialMa();
2023 assertEquals(new Matrix(3, 3), ma1);
2024 final var ma2 = new Matrix(3, 3);
2025 calibrator.getInitialMa(ma2);
2026 assertEquals(ma1, ma2);
2027 assertNull(calibrator.getMeasurements());
2028 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2029 calibrator.getMeasurementType());
2030 assertFalse(calibrator.isOrderedMeasurementsRequired());
2031 assertFalse(calibrator.isQualityScoresRequired());
2032 assertTrue(calibrator.isCommonAxisUsed());
2033 assertNull(calibrator.getListener());
2034 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
2035 assertFalse(calibrator.isReady());
2036 assertFalse(calibrator.isRunning());
2037 assertNull(calibrator.getEstimatedMa());
2038 assertNull(calibrator.getEstimatedSx());
2039 assertNull(calibrator.getEstimatedSy());
2040 assertNull(calibrator.getEstimatedSz());
2041 assertNull(calibrator.getEstimatedMxy());
2042 assertNull(calibrator.getEstimatedMxz());
2043 assertNull(calibrator.getEstimatedMyx());
2044 assertNull(calibrator.getEstimatedMyz());
2045 assertNull(calibrator.getEstimatedMzx());
2046 assertNull(calibrator.getEstimatedMzy());
2047 assertNull(calibrator.getEstimatedCovariance());
2048 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2049 assertNull(calibrator.getGroundTruthGravityNorm());
2050 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2051 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2052 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2053 }
2054
2055 @Test
2056 void testConstructor22() throws WrongSizeException {
2057 final var ba = generateBa();
2058 final var biasX = ba.getElementAtIndex(0);
2059 final var biasY = ba.getElementAtIndex(1);
2060 final var biasZ = ba.getElementAtIndex(2);
2061
2062 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2063 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2064 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2065
2066 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bx, by, bz,
2067 this);
2068
2069
2070 assertEquals(biasX, calibrator.getBiasX(), 0.0);
2071 assertEquals(biasY, calibrator.getBiasY(), 0.0);
2072 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2073 final var bx1 = calibrator.getBiasXAsAcceleration();
2074 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2075 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2076 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2077 calibrator.getBiasXAsAcceleration(bx2);
2078 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2079 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2080 final var by1 = calibrator.getBiasYAsAcceleration();
2081 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2082 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2083 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2084 calibrator.getBiasYAsAcceleration(by2);
2085 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2086 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2087 final var bz1 = calibrator.getBiasZAsAcceleration();
2088 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2089 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2090 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2091 calibrator.getBiasZAsAcceleration(bz2);
2092 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2093 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2094 final var biasTriad1 = calibrator.getBiasAsTriad();
2095 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2096 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2097 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2098 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2099 final var biasTriad2 = new AccelerationTriad();
2100 calibrator.getBiasAsTriad(biasTriad2);
2101 assertEquals(biasTriad1, biasTriad2);
2102 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
2103 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
2104 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
2105 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2106 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2107 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2108 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2109 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2110 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2111 final var bias1 = calibrator.getBias();
2112 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2113 final var bias2 = new double[3];
2114 calibrator.getBias(bias2);
2115 assertArrayEquals(bias1, bias2, 0.0);
2116 final var b1 = calibrator.getBiasAsMatrix();
2117 assertEquals(b1, ba);
2118 final var b2 = new Matrix(3, 1);
2119 calibrator.getBiasAsMatrix(b2);
2120 assertEquals(b1, b2);
2121 final var ma1 = calibrator.getInitialMa();
2122 assertEquals(new Matrix(3, 3), ma1);
2123 final var ma2 = new Matrix(3, 3);
2124 calibrator.getInitialMa(ma2);
2125 assertEquals(ma1, ma2);
2126 assertNull(calibrator.getMeasurements());
2127 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2128 calibrator.getMeasurementType());
2129 assertFalse(calibrator.isOrderedMeasurementsRequired());
2130 assertFalse(calibrator.isQualityScoresRequired());
2131 assertTrue(calibrator.isCommonAxisUsed());
2132 assertSame(this, calibrator.getListener());
2133 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
2134 assertFalse(calibrator.isReady());
2135 assertFalse(calibrator.isRunning());
2136 assertNull(calibrator.getEstimatedMa());
2137 assertNull(calibrator.getEstimatedSx());
2138 assertNull(calibrator.getEstimatedSy());
2139 assertNull(calibrator.getEstimatedSz());
2140 assertNull(calibrator.getEstimatedMxy());
2141 assertNull(calibrator.getEstimatedMxz());
2142 assertNull(calibrator.getEstimatedMyx());
2143 assertNull(calibrator.getEstimatedMyz());
2144 assertNull(calibrator.getEstimatedMzx());
2145 assertNull(calibrator.getEstimatedMzy());
2146 assertNull(calibrator.getEstimatedCovariance());
2147 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2148 assertNull(calibrator.getGroundTruthGravityNorm());
2149 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2150 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2151 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2152 }
2153
2154 @Test
2155 void testConstructor23() throws WrongSizeException {
2156 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
2157
2158 final var ba = generateBa();
2159 final var biasX = ba.getElementAtIndex(0);
2160 final var biasY = ba.getElementAtIndex(1);
2161 final var biasZ = ba.getElementAtIndex(2);
2162
2163 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2164 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2165 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2166
2167 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
2168 bx, by, bz);
2169
2170
2171 assertEquals(biasX, calibrator.getBiasX(), 0.0);
2172 assertEquals(biasY, calibrator.getBiasY(), 0.0);
2173 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2174 final var bx1 = calibrator.getBiasXAsAcceleration();
2175 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2176 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2177 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2178 calibrator.getBiasXAsAcceleration(bx2);
2179 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2180 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2181 final var by1 = calibrator.getBiasYAsAcceleration();
2182 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2183 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2184 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2185 calibrator.getBiasYAsAcceleration(by2);
2186 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2187 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2188 final var bz1 = calibrator.getBiasZAsAcceleration();
2189 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2190 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2191 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2192 calibrator.getBiasZAsAcceleration(bz2);
2193 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2194 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2195 final var biasTriad1 = calibrator.getBiasAsTriad();
2196 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2197 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2198 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2199 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2200 final var biasTriad2 = new AccelerationTriad();
2201 calibrator.getBiasAsTriad(biasTriad2);
2202 assertEquals(biasTriad1, biasTriad2);
2203 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
2204 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
2205 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
2206 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2207 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2208 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2209 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2210 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2211 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2212 final var bias1 = calibrator.getBias();
2213 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2214 final var bias2 = new double[3];
2215 calibrator.getBias(bias2);
2216 assertArrayEquals(bias1, bias2, 0.0);
2217 final var b1 = calibrator.getBiasAsMatrix();
2218 assertEquals(b1, ba);
2219 final var b2 = new Matrix(3, 1);
2220 calibrator.getBiasAsMatrix(b2);
2221 assertEquals(b1, b2);
2222 final var ma1 = calibrator.getInitialMa();
2223 assertEquals(new Matrix(3, 3), ma1);
2224 final var ma2 = new Matrix(3, 3);
2225 calibrator.getInitialMa(ma2);
2226 assertEquals(ma1, ma2);
2227 assertSame(measurements, calibrator.getMeasurements());
2228 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2229 calibrator.getMeasurementType());
2230 assertFalse(calibrator.isOrderedMeasurementsRequired());
2231 assertFalse(calibrator.isQualityScoresRequired());
2232 assertTrue(calibrator.isCommonAxisUsed());
2233 assertNull(calibrator.getListener());
2234 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
2235 assertFalse(calibrator.isReady());
2236 assertFalse(calibrator.isRunning());
2237 assertNull(calibrator.getEstimatedMa());
2238 assertNull(calibrator.getEstimatedSx());
2239 assertNull(calibrator.getEstimatedSy());
2240 assertNull(calibrator.getEstimatedSz());
2241 assertNull(calibrator.getEstimatedMxy());
2242 assertNull(calibrator.getEstimatedMxz());
2243 assertNull(calibrator.getEstimatedMyx());
2244 assertNull(calibrator.getEstimatedMyz());
2245 assertNull(calibrator.getEstimatedMzx());
2246 assertNull(calibrator.getEstimatedMzy());
2247 assertNull(calibrator.getEstimatedCovariance());
2248 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2249 assertNull(calibrator.getGroundTruthGravityNorm());
2250 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2251 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2252 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2253 }
2254
2255 @Test
2256 void testConstructor24() throws WrongSizeException {
2257 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
2258
2259 final var ba = generateBa();
2260 final var biasX = ba.getElementAtIndex(0);
2261 final var biasY = ba.getElementAtIndex(1);
2262 final var biasZ = ba.getElementAtIndex(2);
2263
2264 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2265 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2266 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2267
2268 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
2269 bx, by, bz, this);
2270
2271
2272 assertEquals(biasX, calibrator.getBiasX(), 0.0);
2273 assertEquals(biasY, calibrator.getBiasY(), 0.0);
2274 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2275 final var bx1 = calibrator.getBiasXAsAcceleration();
2276 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2277 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2278 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2279 calibrator.getBiasXAsAcceleration(bx2);
2280 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2281 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2282 final var by1 = calibrator.getBiasYAsAcceleration();
2283 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2284 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2285 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2286 calibrator.getBiasYAsAcceleration(by2);
2287 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2288 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2289 final var bz1 = calibrator.getBiasZAsAcceleration();
2290 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2291 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2292 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2293 calibrator.getBiasZAsAcceleration(bz2);
2294 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2295 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2296 final var biasTriad1 = calibrator.getBiasAsTriad();
2297 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2298 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2299 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2300 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2301 final var biasTriad2 = new AccelerationTriad();
2302 calibrator.getBiasAsTriad(biasTriad2);
2303 assertEquals(biasTriad1, biasTriad2);
2304 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
2305 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
2306 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
2307 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2308 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2309 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2310 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2311 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2312 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2313 final var bias1 = calibrator.getBias();
2314 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2315 final var bias2 = new double[3];
2316 calibrator.getBias(bias2);
2317 assertArrayEquals(bias1, bias2, 0.0);
2318 final var b1 = calibrator.getBiasAsMatrix();
2319 assertEquals(b1, ba);
2320 final var b2 = new Matrix(3, 1);
2321 calibrator.getBiasAsMatrix(b2);
2322 assertEquals(b1, b2);
2323 final var ma1 = calibrator.getInitialMa();
2324 assertEquals(new Matrix(3, 3), ma1);
2325 final var ma2 = new Matrix(3, 3);
2326 calibrator.getInitialMa(ma2);
2327 assertEquals(ma1, ma2);
2328 assertSame(measurements, calibrator.getMeasurements());
2329 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2330 calibrator.getMeasurementType());
2331 assertFalse(calibrator.isOrderedMeasurementsRequired());
2332 assertFalse(calibrator.isQualityScoresRequired());
2333 assertTrue(calibrator.isCommonAxisUsed());
2334 assertSame(this, calibrator.getListener());
2335 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
2336 assertFalse(calibrator.isReady());
2337 assertFalse(calibrator.isRunning());
2338 assertNull(calibrator.getEstimatedMa());
2339 assertNull(calibrator.getEstimatedSx());
2340 assertNull(calibrator.getEstimatedSy());
2341 assertNull(calibrator.getEstimatedSz());
2342 assertNull(calibrator.getEstimatedMxy());
2343 assertNull(calibrator.getEstimatedMxz());
2344 assertNull(calibrator.getEstimatedMyx());
2345 assertNull(calibrator.getEstimatedMyz());
2346 assertNull(calibrator.getEstimatedMzx());
2347 assertNull(calibrator.getEstimatedMzy());
2348 assertNull(calibrator.getEstimatedCovariance());
2349 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2350 assertNull(calibrator.getGroundTruthGravityNorm());
2351 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2352 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2353 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2354 }
2355
2356 @Test
2357 void testConstructor25() throws WrongSizeException {
2358 final var ba = generateBa();
2359 final var biasX = ba.getElementAtIndex(0);
2360 final var biasY = ba.getElementAtIndex(1);
2361 final var biasZ = ba.getElementAtIndex(2);
2362
2363 final var ma = generateMaCommonAxis();
2364 final var sx = ma.getElementAt(0, 0);
2365 final var sy = ma.getElementAt(1, 1);
2366 final var sz = ma.getElementAt(2, 2);
2367
2368 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(biasX, biasY, biasZ, sx, sy, sz);
2369
2370
2371 assertEquals(biasX, calibrator.getBiasX(), 0.0);
2372 assertEquals(biasY, calibrator.getBiasY(), 0.0);
2373 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2374 final var bx1 = calibrator.getBiasXAsAcceleration();
2375 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2376 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2377 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2378 calibrator.getBiasXAsAcceleration(bx2);
2379 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2380 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2381 final var by1 = calibrator.getBiasYAsAcceleration();
2382 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2383 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2384 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2385 calibrator.getBiasYAsAcceleration(by2);
2386 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2387 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2388 final var bz1 = calibrator.getBiasZAsAcceleration();
2389 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2390 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2391 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2392 calibrator.getBiasZAsAcceleration(bz2);
2393 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2394 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2395 final var biasTriad1 = calibrator.getBiasAsTriad();
2396 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2397 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2398 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2399 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2400 final var biasTriad2 = new AccelerationTriad();
2401 calibrator.getBiasAsTriad(biasTriad2);
2402 assertEquals(biasTriad1, biasTriad2);
2403 assertEquals(sx, calibrator.getInitialSx(), 0.0);
2404 assertEquals(sy, calibrator.getInitialSy(), 0.0);
2405 assertEquals(sz, calibrator.getInitialSz(), 0.0);
2406 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2407 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2408 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2409 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2410 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2411 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2412 final var bias1 = calibrator.getBias();
2413 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2414 final var bias2 = new double[3];
2415 calibrator.getBias(bias2);
2416 assertArrayEquals(bias1, bias2, 0.0);
2417 final var b1 = calibrator.getBiasAsMatrix();
2418 assertEquals(b1, ba);
2419 final var b2 = new Matrix(3, 1);
2420 calibrator.getBiasAsMatrix(b2);
2421 assertEquals(b1, b2);
2422 final var ma1 = calibrator.getInitialMa();
2423 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2424 final var ma2 = new Matrix(3, 3);
2425 calibrator.getInitialMa(ma2);
2426 assertEquals(ma1, ma2);
2427 assertNull(calibrator.getMeasurements());
2428 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2429 calibrator.getMeasurementType());
2430 assertFalse(calibrator.isOrderedMeasurementsRequired());
2431 assertFalse(calibrator.isQualityScoresRequired());
2432 assertFalse(calibrator.isCommonAxisUsed());
2433 assertNull(calibrator.getListener());
2434 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
2435 assertFalse(calibrator.isReady());
2436 assertFalse(calibrator.isRunning());
2437 assertNull(calibrator.getEstimatedMa());
2438 assertNull(calibrator.getEstimatedSx());
2439 assertNull(calibrator.getEstimatedSy());
2440 assertNull(calibrator.getEstimatedSz());
2441 assertNull(calibrator.getEstimatedMxy());
2442 assertNull(calibrator.getEstimatedMxz());
2443 assertNull(calibrator.getEstimatedMyx());
2444 assertNull(calibrator.getEstimatedMyz());
2445 assertNull(calibrator.getEstimatedMzx());
2446 assertNull(calibrator.getEstimatedMzy());
2447 assertNull(calibrator.getEstimatedCovariance());
2448 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2449 assertNull(calibrator.getGroundTruthGravityNorm());
2450 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2451 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2452 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2453 }
2454
2455 @Test
2456 void testConstructor26() throws WrongSizeException {
2457 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
2458
2459 final var ba = generateBa();
2460 final var biasX = ba.getElementAtIndex(0);
2461 final var biasY = ba.getElementAtIndex(1);
2462 final var biasZ = ba.getElementAtIndex(2);
2463
2464 final var ma = generateMaCommonAxis();
2465 final var sx = ma.getElementAt(0, 0);
2466 final var sy = ma.getElementAt(1, 1);
2467 final var sz = ma.getElementAt(2, 2);
2468
2469 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
2470 sx, sy, sz);
2471
2472
2473 assertEquals(biasX, calibrator.getBiasX(), 0.0);
2474 assertEquals(biasY, calibrator.getBiasY(), 0.0);
2475 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2476 final var bx1 = calibrator.getBiasXAsAcceleration();
2477 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2478 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2479 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2480 calibrator.getBiasXAsAcceleration(bx2);
2481 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2482 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2483 final var by1 = calibrator.getBiasYAsAcceleration();
2484 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2485 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2486 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2487 calibrator.getBiasYAsAcceleration(by2);
2488 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2489 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2490 final var bz1 = calibrator.getBiasZAsAcceleration();
2491 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2492 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2493 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2494 calibrator.getBiasZAsAcceleration(bz2);
2495 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2496 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2497 final var biasTriad1 = calibrator.getBiasAsTriad();
2498 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2499 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2500 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2501 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2502 final var biasTriad2 = new AccelerationTriad();
2503 calibrator.getBiasAsTriad(biasTriad2);
2504 assertEquals(biasTriad1, biasTriad2);
2505 assertEquals(sx, calibrator.getInitialSx(), 0.0);
2506 assertEquals(sy, calibrator.getInitialSy(), 0.0);
2507 assertEquals(sz, calibrator.getInitialSz(), 0.0);
2508 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2509 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2510 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2511 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2512 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2513 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2514 final var bias1 = calibrator.getBias();
2515 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2516 final var bias2 = new double[3];
2517 calibrator.getBias(bias2);
2518 assertArrayEquals(bias1, bias2, 0.0);
2519 final var b1 = calibrator.getBiasAsMatrix();
2520 assertEquals(b1, ba);
2521 final var b2 = new Matrix(3, 1);
2522 calibrator.getBiasAsMatrix(b2);
2523 assertEquals(b1, b2);
2524 final var ma1 = calibrator.getInitialMa();
2525 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2526 final var ma2 = new Matrix(3, 3);
2527 calibrator.getInitialMa(ma2);
2528 assertEquals(ma1, ma2);
2529 assertSame(measurements, calibrator.getMeasurements());
2530 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2531 calibrator.getMeasurementType());
2532 assertFalse(calibrator.isOrderedMeasurementsRequired());
2533 assertFalse(calibrator.isQualityScoresRequired());
2534 assertFalse(calibrator.isCommonAxisUsed());
2535 assertNull(calibrator.getListener());
2536 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
2537 assertFalse(calibrator.isReady());
2538 assertFalse(calibrator.isRunning());
2539 assertNull(calibrator.getEstimatedMa());
2540 assertNull(calibrator.getEstimatedSx());
2541 assertNull(calibrator.getEstimatedSy());
2542 assertNull(calibrator.getEstimatedSz());
2543 assertNull(calibrator.getEstimatedMxy());
2544 assertNull(calibrator.getEstimatedMxz());
2545 assertNull(calibrator.getEstimatedMyx());
2546 assertNull(calibrator.getEstimatedMyz());
2547 assertNull(calibrator.getEstimatedMzx());
2548 assertNull(calibrator.getEstimatedMzy());
2549 assertNull(calibrator.getEstimatedCovariance());
2550 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2551 assertNull(calibrator.getGroundTruthGravityNorm());
2552 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2553 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2554 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2555 }
2556
2557 @Test
2558 void testConstructor27() throws WrongSizeException {
2559 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
2560
2561 final var ba = generateBa();
2562 final var biasX = ba.getElementAtIndex(0);
2563 final var biasY = ba.getElementAtIndex(1);
2564 final var biasZ = ba.getElementAtIndex(2);
2565
2566 final var ma = generateMaCommonAxis();
2567 final var sx = ma.getElementAt(0, 0);
2568 final var sy = ma.getElementAt(1, 1);
2569 final var sz = ma.getElementAt(2, 2);
2570
2571 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
2572 sx, sy, sz, this);
2573
2574
2575 assertEquals(biasX, calibrator.getBiasX(), 0.0);
2576 assertEquals(biasY, calibrator.getBiasY(), 0.0);
2577 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2578 final var bx1 = calibrator.getBiasXAsAcceleration();
2579 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2580 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2581 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2582 calibrator.getBiasXAsAcceleration(bx2);
2583 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2584 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2585 final var by1 = calibrator.getBiasYAsAcceleration();
2586 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2587 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2588 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2589 calibrator.getBiasYAsAcceleration(by2);
2590 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2591 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2592 final var bz1 = calibrator.getBiasZAsAcceleration();
2593 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2594 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2595 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2596 calibrator.getBiasZAsAcceleration(bz2);
2597 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2598 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2599 final var biasTriad1 = calibrator.getBiasAsTriad();
2600 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2601 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2602 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2603 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2604 final var biasTriad2 = new AccelerationTriad();
2605 calibrator.getBiasAsTriad(biasTriad2);
2606 assertEquals(biasTriad1, biasTriad2);
2607 assertEquals(sx, calibrator.getInitialSx(), 0.0);
2608 assertEquals(sy, calibrator.getInitialSy(), 0.0);
2609 assertEquals(sz, calibrator.getInitialSz(), 0.0);
2610 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2611 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2612 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2613 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2614 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2615 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2616 final var bias1 = calibrator.getBias();
2617 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2618 final var bias2 = new double[3];
2619 calibrator.getBias(bias2);
2620 assertArrayEquals(bias1, bias2, 0.0);
2621 final var b1 = calibrator.getBiasAsMatrix();
2622 assertEquals(b1, ba);
2623 final var b2 = new Matrix(3, 1);
2624 calibrator.getBiasAsMatrix(b2);
2625 assertEquals(b1, b2);
2626 final var ma1 = calibrator.getInitialMa();
2627 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2628 final var ma2 = new Matrix(3, 3);
2629 calibrator.getInitialMa(ma2);
2630 assertEquals(ma1, ma2);
2631 assertSame(measurements, calibrator.getMeasurements());
2632 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2633 calibrator.getMeasurementType());
2634 assertFalse(calibrator.isOrderedMeasurementsRequired());
2635 assertFalse(calibrator.isQualityScoresRequired());
2636 assertFalse(calibrator.isCommonAxisUsed());
2637 assertSame(this, calibrator.getListener());
2638 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
2639 assertFalse(calibrator.isReady());
2640 assertFalse(calibrator.isRunning());
2641 assertNull(calibrator.getEstimatedMa());
2642 assertNull(calibrator.getEstimatedSx());
2643 assertNull(calibrator.getEstimatedSy());
2644 assertNull(calibrator.getEstimatedSz());
2645 assertNull(calibrator.getEstimatedMxy());
2646 assertNull(calibrator.getEstimatedMxz());
2647 assertNull(calibrator.getEstimatedMyx());
2648 assertNull(calibrator.getEstimatedMyz());
2649 assertNull(calibrator.getEstimatedMzx());
2650 assertNull(calibrator.getEstimatedMzy());
2651 assertNull(calibrator.getEstimatedCovariance());
2652 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2653 assertNull(calibrator.getGroundTruthGravityNorm());
2654 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2655 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2656 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2657 }
2658
2659 @Test
2660 void testConstructor28() throws WrongSizeException {
2661 final var ba = generateBa();
2662 final var biasX = ba.getElementAtIndex(0);
2663 final var biasY = ba.getElementAtIndex(1);
2664 final var biasZ = ba.getElementAtIndex(2);
2665
2666 final var ma = generateMaCommonAxis();
2667 final var sx = ma.getElementAt(0, 0);
2668 final var sy = ma.getElementAt(1, 1);
2669 final var sz = ma.getElementAt(2, 2);
2670
2671 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true,
2672 biasX, biasY, biasZ, sx, sy, sz);
2673
2674
2675 assertEquals(biasX, calibrator.getBiasX(), 0.0);
2676 assertEquals(biasY, calibrator.getBiasY(), 0.0);
2677 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2678 final var bx1 = calibrator.getBiasXAsAcceleration();
2679 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2680 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2681 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2682 calibrator.getBiasXAsAcceleration(bx2);
2683 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2684 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2685 final var by1 = calibrator.getBiasYAsAcceleration();
2686 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2687 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2688 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2689 calibrator.getBiasYAsAcceleration(by2);
2690 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2691 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2692 final var bz1 = calibrator.getBiasZAsAcceleration();
2693 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2694 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2695 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2696 calibrator.getBiasZAsAcceleration(bz2);
2697 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2698 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2699 final var biasTriad1 = calibrator.getBiasAsTriad();
2700 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2701 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2702 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2703 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2704 final var biasTriad2 = new AccelerationTriad();
2705 calibrator.getBiasAsTriad(biasTriad2);
2706 assertEquals(biasTriad1, biasTriad2);
2707 assertEquals(sx, calibrator.getInitialSx(), 0.0);
2708 assertEquals(sy, calibrator.getInitialSy(), 0.0);
2709 assertEquals(sz, calibrator.getInitialSz(), 0.0);
2710 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2711 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2712 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2713 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2714 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2715 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2716 final var bias1 = calibrator.getBias();
2717 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2718 final var bias2 = new double[3];
2719 calibrator.getBias(bias2);
2720 assertArrayEquals(bias1, bias2, 0.0);
2721 final var b1 = calibrator.getBiasAsMatrix();
2722 assertEquals(b1, ba);
2723 final var b2 = new Matrix(3, 1);
2724 calibrator.getBiasAsMatrix(b2);
2725 assertEquals(b1, b2);
2726 final var ma1 = calibrator.getInitialMa();
2727 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2728 final var ma2 = new Matrix(3, 3);
2729 calibrator.getInitialMa(ma2);
2730 assertEquals(ma1, ma2);
2731 assertNull(calibrator.getMeasurements());
2732 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2733 calibrator.getMeasurementType());
2734 assertFalse(calibrator.isOrderedMeasurementsRequired());
2735 assertFalse(calibrator.isQualityScoresRequired());
2736 assertTrue(calibrator.isCommonAxisUsed());
2737 assertNull(calibrator.getListener());
2738 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
2739 assertFalse(calibrator.isReady());
2740 assertFalse(calibrator.isRunning());
2741 assertNull(calibrator.getEstimatedMa());
2742 assertNull(calibrator.getEstimatedSx());
2743 assertNull(calibrator.getEstimatedSy());
2744 assertNull(calibrator.getEstimatedSz());
2745 assertNull(calibrator.getEstimatedMxy());
2746 assertNull(calibrator.getEstimatedMxz());
2747 assertNull(calibrator.getEstimatedMyx());
2748 assertNull(calibrator.getEstimatedMyz());
2749 assertNull(calibrator.getEstimatedMzx());
2750 assertNull(calibrator.getEstimatedMzy());
2751 assertNull(calibrator.getEstimatedCovariance());
2752 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2753 assertNull(calibrator.getGroundTruthGravityNorm());
2754 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2755 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2756 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2757 }
2758
2759 @Test
2760 void testConstructor29() throws WrongSizeException {
2761 final var ba = generateBa();
2762 final var biasX = ba.getElementAtIndex(0);
2763 final var biasY = ba.getElementAtIndex(1);
2764 final var biasZ = ba.getElementAtIndex(2);
2765
2766 final var ma = generateMaCommonAxis();
2767 final var sx = ma.getElementAt(0, 0);
2768 final var sy = ma.getElementAt(1, 1);
2769 final var sz = ma.getElementAt(2, 2);
2770
2771 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true,
2772 biasX, biasY, biasZ, sx, sy, sz, this);
2773
2774
2775 assertEquals(biasX, calibrator.getBiasX(), 0.0);
2776 assertEquals(biasY, calibrator.getBiasY(), 0.0);
2777 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2778 final var bx1 = calibrator.getBiasXAsAcceleration();
2779 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2780 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2781 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2782 calibrator.getBiasXAsAcceleration(bx2);
2783 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2784 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2785 final var by1 = calibrator.getBiasYAsAcceleration();
2786 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2787 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2788 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2789 calibrator.getBiasYAsAcceleration(by2);
2790 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2791 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2792 final var bz1 = calibrator.getBiasZAsAcceleration();
2793 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2794 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2795 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2796 calibrator.getBiasZAsAcceleration(bz2);
2797 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2798 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2799 final var biasTriad1 = calibrator.getBiasAsTriad();
2800 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2801 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2802 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2803 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2804 final var biasTriad2 = new AccelerationTriad();
2805 calibrator.getBiasAsTriad(biasTriad2);
2806 assertEquals(biasTriad1, biasTriad2);
2807 assertEquals(sx, calibrator.getInitialSx(), 0.0);
2808 assertEquals(sy, calibrator.getInitialSy(), 0.0);
2809 assertEquals(sz, calibrator.getInitialSz(), 0.0);
2810 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2811 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2812 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2813 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2814 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2815 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2816 final var bias1 = calibrator.getBias();
2817 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2818 final var bias2 = new double[3];
2819 calibrator.getBias(bias2);
2820 assertArrayEquals(bias1, bias2, 0.0);
2821 final var b1 = calibrator.getBiasAsMatrix();
2822 assertEquals(b1, ba);
2823 final var b2 = new Matrix(3, 1);
2824 calibrator.getBiasAsMatrix(b2);
2825 assertEquals(b1, b2);
2826 final var ma1 = calibrator.getInitialMa();
2827 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2828 final var ma2 = new Matrix(3, 3);
2829 calibrator.getInitialMa(ma2);
2830 assertEquals(ma1, ma2);
2831 assertNull(calibrator.getMeasurements());
2832 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2833 calibrator.getMeasurementType());
2834 assertFalse(calibrator.isOrderedMeasurementsRequired());
2835 assertFalse(calibrator.isQualityScoresRequired());
2836 assertTrue(calibrator.isCommonAxisUsed());
2837 assertSame(this, calibrator.getListener());
2838 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
2839 assertFalse(calibrator.isReady());
2840 assertFalse(calibrator.isRunning());
2841 assertNull(calibrator.getEstimatedMa());
2842 assertNull(calibrator.getEstimatedSx());
2843 assertNull(calibrator.getEstimatedSy());
2844 assertNull(calibrator.getEstimatedSz());
2845 assertNull(calibrator.getEstimatedMxy());
2846 assertNull(calibrator.getEstimatedMxz());
2847 assertNull(calibrator.getEstimatedMyx());
2848 assertNull(calibrator.getEstimatedMyz());
2849 assertNull(calibrator.getEstimatedMzx());
2850 assertNull(calibrator.getEstimatedMzy());
2851 assertNull(calibrator.getEstimatedCovariance());
2852 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2853 assertNull(calibrator.getGroundTruthGravityNorm());
2854 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2855 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2856 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2857 }
2858
2859 @Test
2860 void testConstructor30() throws WrongSizeException {
2861 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
2862 final var ba = generateBa();
2863 final var biasX = ba.getElementAtIndex(0);
2864 final var biasY = ba.getElementAtIndex(1);
2865 final var biasZ = ba.getElementAtIndex(2);
2866
2867 final var ma = generateMaCommonAxis();
2868 final var sx = ma.getElementAt(0, 0);
2869 final var sy = ma.getElementAt(1, 1);
2870 final var sz = ma.getElementAt(2, 2);
2871
2872 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
2873 biasX, biasY, biasZ, sx, sy, sz);
2874
2875
2876 assertEquals(biasX, calibrator.getBiasX(), 0.0);
2877 assertEquals(biasY, calibrator.getBiasY(), 0.0);
2878 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2879 final var bx1 = calibrator.getBiasXAsAcceleration();
2880 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2881 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2882 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2883 calibrator.getBiasXAsAcceleration(bx2);
2884 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2885 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2886 final var by1 = calibrator.getBiasYAsAcceleration();
2887 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2888 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2889 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2890 calibrator.getBiasYAsAcceleration(by2);
2891 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2892 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2893 final var bz1 = calibrator.getBiasZAsAcceleration();
2894 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2895 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2896 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2897 calibrator.getBiasZAsAcceleration(bz2);
2898 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2899 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2900 final var biasTriad1 = calibrator.getBiasAsTriad();
2901 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2902 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2903 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2904 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2905 final var biasTriad2 = new AccelerationTriad();
2906 calibrator.getBiasAsTriad(biasTriad2);
2907 assertEquals(biasTriad1, biasTriad2);
2908 assertEquals(sx, calibrator.getInitialSx(), 0.0);
2909 assertEquals(sy, calibrator.getInitialSy(), 0.0);
2910 assertEquals(sz, calibrator.getInitialSz(), 0.0);
2911 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2912 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2913 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2914 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2915 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2916 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2917 final var bias1 = calibrator.getBias();
2918 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2919 final var bias2 = new double[3];
2920 calibrator.getBias(bias2);
2921 assertArrayEquals(bias1, bias2, 0.0);
2922 final var b1 = calibrator.getBiasAsMatrix();
2923 assertEquals(b1, ba);
2924 final var b2 = new Matrix(3, 1);
2925 calibrator.getBiasAsMatrix(b2);
2926 assertEquals(b1, b2);
2927 final var ma1 = calibrator.getInitialMa();
2928 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2929 final var ma2 = new Matrix(3, 3);
2930 calibrator.getInitialMa(ma2);
2931 assertEquals(ma1, ma2);
2932 assertSame(measurements, calibrator.getMeasurements());
2933 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2934 calibrator.getMeasurementType());
2935 assertFalse(calibrator.isOrderedMeasurementsRequired());
2936 assertFalse(calibrator.isQualityScoresRequired());
2937 assertTrue(calibrator.isCommonAxisUsed());
2938 assertNull(calibrator.getListener());
2939 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
2940 assertFalse(calibrator.isReady());
2941 assertFalse(calibrator.isRunning());
2942 assertNull(calibrator.getEstimatedMa());
2943 assertNull(calibrator.getEstimatedSx());
2944 assertNull(calibrator.getEstimatedSy());
2945 assertNull(calibrator.getEstimatedSz());
2946 assertNull(calibrator.getEstimatedMxy());
2947 assertNull(calibrator.getEstimatedMxz());
2948 assertNull(calibrator.getEstimatedMyx());
2949 assertNull(calibrator.getEstimatedMyz());
2950 assertNull(calibrator.getEstimatedMzx());
2951 assertNull(calibrator.getEstimatedMzy());
2952 assertNull(calibrator.getEstimatedCovariance());
2953 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2954 assertNull(calibrator.getGroundTruthGravityNorm());
2955 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2956 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2957 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2958 }
2959
2960 @Test
2961 void testConstructor31() throws WrongSizeException {
2962 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
2963 final var ba = generateBa();
2964 final var biasX = ba.getElementAtIndex(0);
2965 final var biasY = ba.getElementAtIndex(1);
2966 final var biasZ = ba.getElementAtIndex(2);
2967
2968 final var ma = generateMaCommonAxis();
2969 final var sx = ma.getElementAt(0, 0);
2970 final var sy = ma.getElementAt(1, 1);
2971 final var sz = ma.getElementAt(2, 2);
2972
2973 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
2974 biasX, biasY, biasZ, sx, sy, sz, this);
2975
2976
2977 assertEquals(biasX, calibrator.getBiasX(), 0.0);
2978 assertEquals(biasY, calibrator.getBiasY(), 0.0);
2979 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2980 final var bx1 = calibrator.getBiasXAsAcceleration();
2981 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2982 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2983 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2984 calibrator.getBiasXAsAcceleration(bx2);
2985 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2986 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2987 final var by1 = calibrator.getBiasYAsAcceleration();
2988 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2989 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2990 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2991 calibrator.getBiasYAsAcceleration(by2);
2992 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2993 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2994 final var bz1 = calibrator.getBiasZAsAcceleration();
2995 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2996 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2997 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2998 calibrator.getBiasZAsAcceleration(bz2);
2999 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3000 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3001 final var biasTriad1 = calibrator.getBiasAsTriad();
3002 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3003 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3004 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3005 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3006 final var biasTriad2 = new AccelerationTriad();
3007 calibrator.getBiasAsTriad(biasTriad2);
3008 assertEquals(biasTriad1, biasTriad2);
3009 assertEquals(sx, calibrator.getInitialSx(), 0.0);
3010 assertEquals(sy, calibrator.getInitialSy(), 0.0);
3011 assertEquals(sz, calibrator.getInitialSz(), 0.0);
3012 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3013 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3014 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3015 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3016 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3017 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3018 final var bias1 = calibrator.getBias();
3019 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3020 final var bias2 = new double[3];
3021 calibrator.getBias(bias2);
3022 assertArrayEquals(bias1, bias2, 0.0);
3023 final var b1 = calibrator.getBiasAsMatrix();
3024 assertEquals(b1, ba);
3025 final var b2 = new Matrix(3, 1);
3026 calibrator.getBiasAsMatrix(b2);
3027 assertEquals(b1, b2);
3028 final var ma1 = calibrator.getInitialMa();
3029 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3030 final var ma2 = new Matrix(3, 3);
3031 calibrator.getInitialMa(ma2);
3032 assertEquals(ma1, ma2);
3033 assertSame(measurements, calibrator.getMeasurements());
3034 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3035 calibrator.getMeasurementType());
3036 assertFalse(calibrator.isOrderedMeasurementsRequired());
3037 assertFalse(calibrator.isQualityScoresRequired());
3038 assertTrue(calibrator.isCommonAxisUsed());
3039 assertSame(this, calibrator.getListener());
3040 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
3041 assertFalse(calibrator.isReady());
3042 assertFalse(calibrator.isRunning());
3043 assertNull(calibrator.getEstimatedMa());
3044 assertNull(calibrator.getEstimatedSx());
3045 assertNull(calibrator.getEstimatedSy());
3046 assertNull(calibrator.getEstimatedSz());
3047 assertNull(calibrator.getEstimatedMxy());
3048 assertNull(calibrator.getEstimatedMxz());
3049 assertNull(calibrator.getEstimatedMyx());
3050 assertNull(calibrator.getEstimatedMyz());
3051 assertNull(calibrator.getEstimatedMzx());
3052 assertNull(calibrator.getEstimatedMzy());
3053 assertNull(calibrator.getEstimatedCovariance());
3054 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3055 assertNull(calibrator.getGroundTruthGravityNorm());
3056 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3057 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3058 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3059 }
3060
3061 @Test
3062 void testConstructor32() throws WrongSizeException {
3063 final var ba = generateBa();
3064 final var biasX = ba.getElementAtIndex(0);
3065 final var biasY = ba.getElementAtIndex(1);
3066 final var biasZ = ba.getElementAtIndex(2);
3067
3068 final var ma = generateMaCommonAxis();
3069 final var sx = ma.getElementAt(0, 0);
3070 final var sy = ma.getElementAt(1, 1);
3071 final var sz = ma.getElementAt(2, 2);
3072
3073 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3074 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3075 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3076
3077 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz, sx, sy, sz);
3078
3079
3080 assertEquals(biasX, calibrator.getBiasX(), 0.0);
3081 assertEquals(biasY, calibrator.getBiasY(), 0.0);
3082 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3083 final var bx1 = calibrator.getBiasXAsAcceleration();
3084 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3085 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3086 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3087 calibrator.getBiasXAsAcceleration(bx2);
3088 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3089 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3090 final var by1 = calibrator.getBiasYAsAcceleration();
3091 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3092 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3093 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3094 calibrator.getBiasYAsAcceleration(by2);
3095 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3096 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3097 final var bz1 = calibrator.getBiasZAsAcceleration();
3098 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3099 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3100 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3101 calibrator.getBiasZAsAcceleration(bz2);
3102 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3103 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3104 final var biasTriad1 = calibrator.getBiasAsTriad();
3105 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3106 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3107 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3108 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3109 final var biasTriad2 = new AccelerationTriad();
3110 calibrator.getBiasAsTriad(biasTriad2);
3111 assertEquals(biasTriad1, biasTriad2);
3112 assertEquals(sx, calibrator.getInitialSx(), 0.0);
3113 assertEquals(sy, calibrator.getInitialSy(), 0.0);
3114 assertEquals(sz, calibrator.getInitialSz(), 0.0);
3115 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3116 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3117 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3118 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3119 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3120 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3121 final var bias1 = calibrator.getBias();
3122 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3123 final var bias2 = new double[3];
3124 calibrator.getBias(bias2);
3125 assertArrayEquals(bias1, bias2, 0.0);
3126 final var b1 = calibrator.getBiasAsMatrix();
3127 assertEquals(b1, ba);
3128 final var b2 = new Matrix(3, 1);
3129 calibrator.getBiasAsMatrix(b2);
3130 assertEquals(b1, b2);
3131 final var ma1 = calibrator.getInitialMa();
3132 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3133 final var ma2 = new Matrix(3, 3);
3134 calibrator.getInitialMa(ma2);
3135 assertEquals(ma1, ma2);
3136 assertNull(calibrator.getMeasurements());
3137 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3138 calibrator.getMeasurementType());
3139 assertFalse(calibrator.isOrderedMeasurementsRequired());
3140 assertFalse(calibrator.isQualityScoresRequired());
3141 assertFalse(calibrator.isCommonAxisUsed());
3142 assertNull(calibrator.getListener());
3143 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
3144 assertFalse(calibrator.isReady());
3145 assertFalse(calibrator.isRunning());
3146 assertNull(calibrator.getEstimatedMa());
3147 assertNull(calibrator.getEstimatedSx());
3148 assertNull(calibrator.getEstimatedSy());
3149 assertNull(calibrator.getEstimatedSz());
3150 assertNull(calibrator.getEstimatedMxy());
3151 assertNull(calibrator.getEstimatedMxz());
3152 assertNull(calibrator.getEstimatedMyx());
3153 assertNull(calibrator.getEstimatedMyz());
3154 assertNull(calibrator.getEstimatedMzx());
3155 assertNull(calibrator.getEstimatedMzy());
3156 assertNull(calibrator.getEstimatedCovariance());
3157 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3158 assertNull(calibrator.getGroundTruthGravityNorm());
3159 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3160 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3161 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3162 }
3163
3164 @Test
3165 void testConstructor33() throws WrongSizeException {
3166 final var ba = generateBa();
3167 final var biasX = ba.getElementAtIndex(0);
3168 final var biasY = ba.getElementAtIndex(1);
3169 final var biasZ = ba.getElementAtIndex(2);
3170
3171 final var ma = generateMaCommonAxis();
3172 final var sx = ma.getElementAt(0, 0);
3173 final var sy = ma.getElementAt(1, 1);
3174 final var sz = ma.getElementAt(2, 2);
3175
3176 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3177 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3178 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3179
3180 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz, sx, sy, sz, this);
3181
3182
3183 assertEquals(biasX, calibrator.getBiasX(), 0.0);
3184 assertEquals(biasY, calibrator.getBiasY(), 0.0);
3185 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3186 final var bx1 = calibrator.getBiasXAsAcceleration();
3187 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3188 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3189 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3190 calibrator.getBiasXAsAcceleration(bx2);
3191 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3192 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3193 final var by1 = calibrator.getBiasYAsAcceleration();
3194 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3195 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3196 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3197 calibrator.getBiasYAsAcceleration(by2);
3198 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3199 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3200 final var bz1 = calibrator.getBiasZAsAcceleration();
3201 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3202 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3203 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3204 calibrator.getBiasZAsAcceleration(bz2);
3205 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3206 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3207 final var biasTriad1 = calibrator.getBiasAsTriad();
3208 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3209 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3210 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3211 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3212 final var biasTriad2 = new AccelerationTriad();
3213 calibrator.getBiasAsTriad(biasTriad2);
3214 assertEquals(biasTriad1, biasTriad2);
3215 assertEquals(sx, calibrator.getInitialSx(), 0.0);
3216 assertEquals(sy, calibrator.getInitialSy(), 0.0);
3217 assertEquals(sz, calibrator.getInitialSz(), 0.0);
3218 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3219 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3220 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3221 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3222 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3223 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3224 final var bias1 = calibrator.getBias();
3225 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3226 final var bias2 = new double[3];
3227 calibrator.getBias(bias2);
3228 assertArrayEquals(bias1, bias2, 0.0);
3229 final var b1 = calibrator.getBiasAsMatrix();
3230 assertEquals(b1, ba);
3231 final var b2 = new Matrix(3, 1);
3232 calibrator.getBiasAsMatrix(b2);
3233 assertEquals(b1, b2);
3234 final var ma1 = calibrator.getInitialMa();
3235 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3236 final var ma2 = new Matrix(3, 3);
3237 calibrator.getInitialMa(ma2);
3238 assertEquals(ma1, ma2);
3239 assertNull(calibrator.getMeasurements());
3240 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3241 calibrator.getMeasurementType());
3242 assertFalse(calibrator.isOrderedMeasurementsRequired());
3243 assertFalse(calibrator.isQualityScoresRequired());
3244 assertFalse(calibrator.isCommonAxisUsed());
3245 assertSame(this, calibrator.getListener());
3246 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
3247 assertFalse(calibrator.isReady());
3248 assertFalse(calibrator.isRunning());
3249 assertNull(calibrator.getEstimatedMa());
3250 assertNull(calibrator.getEstimatedSx());
3251 assertNull(calibrator.getEstimatedSy());
3252 assertNull(calibrator.getEstimatedSz());
3253 assertNull(calibrator.getEstimatedMxy());
3254 assertNull(calibrator.getEstimatedMxz());
3255 assertNull(calibrator.getEstimatedMyx());
3256 assertNull(calibrator.getEstimatedMyz());
3257 assertNull(calibrator.getEstimatedMzx());
3258 assertNull(calibrator.getEstimatedMzy());
3259 assertNull(calibrator.getEstimatedCovariance());
3260 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3261 assertNull(calibrator.getGroundTruthGravityNorm());
3262 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3263 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3264 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3265 }
3266
3267 @Test
3268 void testConstructor34() throws WrongSizeException {
3269 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
3270
3271 final var ba = generateBa();
3272 final var biasX = ba.getElementAtIndex(0);
3273 final var biasY = ba.getElementAtIndex(1);
3274 final var biasZ = ba.getElementAtIndex(2);
3275
3276 final var ma = generateMaCommonAxis();
3277 final var sx = ma.getElementAt(0, 0);
3278 final var sy = ma.getElementAt(1, 1);
3279 final var sz = ma.getElementAt(2, 2);
3280
3281 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3282 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3283 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3284
3285 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bx, by, bz, sx, sy, sz);
3286
3287
3288 assertEquals(biasX, calibrator.getBiasX(), 0.0);
3289 assertEquals(biasY, calibrator.getBiasY(), 0.0);
3290 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3291 final var bx1 = calibrator.getBiasXAsAcceleration();
3292 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3293 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3294 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3295 calibrator.getBiasXAsAcceleration(bx2);
3296 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3297 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3298 final var by1 = calibrator.getBiasYAsAcceleration();
3299 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3300 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3301 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3302 calibrator.getBiasYAsAcceleration(by2);
3303 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3304 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3305 final var bz1 = calibrator.getBiasZAsAcceleration();
3306 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3307 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3308 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3309 calibrator.getBiasZAsAcceleration(bz2);
3310 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3311 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3312 final var biasTriad1 = calibrator.getBiasAsTriad();
3313 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3314 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3315 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3316 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3317 final var biasTriad2 = new AccelerationTriad();
3318 calibrator.getBiasAsTriad(biasTriad2);
3319 assertEquals(biasTriad1, biasTriad2);
3320 assertEquals(sx, calibrator.getInitialSx(), 0.0);
3321 assertEquals(sy, calibrator.getInitialSy(), 0.0);
3322 assertEquals(sz, calibrator.getInitialSz(), 0.0);
3323 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3324 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3325 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3326 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3327 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3328 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3329 final var bias1 = calibrator.getBias();
3330 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3331 final var bias2 = new double[3];
3332 calibrator.getBias(bias2);
3333 assertArrayEquals(bias1, bias2, 0.0);
3334 final var b1 = calibrator.getBiasAsMatrix();
3335 assertEquals(b1, ba);
3336 final var b2 = new Matrix(3, 1);
3337 calibrator.getBiasAsMatrix(b2);
3338 assertEquals(b1, b2);
3339 final var ma1 = calibrator.getInitialMa();
3340 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3341 final var ma2 = new Matrix(3, 3);
3342 calibrator.getInitialMa(ma2);
3343 assertEquals(ma1, ma2);
3344 assertSame(measurements, calibrator.getMeasurements());
3345 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3346 calibrator.getMeasurementType());
3347 assertFalse(calibrator.isOrderedMeasurementsRequired());
3348 assertFalse(calibrator.isQualityScoresRequired());
3349 assertFalse(calibrator.isCommonAxisUsed());
3350 assertNull(calibrator.getListener());
3351 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
3352 assertFalse(calibrator.isReady());
3353 assertFalse(calibrator.isRunning());
3354 assertNull(calibrator.getEstimatedMa());
3355 assertNull(calibrator.getEstimatedSx());
3356 assertNull(calibrator.getEstimatedSy());
3357 assertNull(calibrator.getEstimatedSz());
3358 assertNull(calibrator.getEstimatedMxy());
3359 assertNull(calibrator.getEstimatedMxz());
3360 assertNull(calibrator.getEstimatedMyx());
3361 assertNull(calibrator.getEstimatedMyz());
3362 assertNull(calibrator.getEstimatedMzx());
3363 assertNull(calibrator.getEstimatedMzy());
3364 assertNull(calibrator.getEstimatedCovariance());
3365 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3366 assertNull(calibrator.getGroundTruthGravityNorm());
3367 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3368 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3369 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3370 }
3371
3372 @Test
3373 void testConstructor35() throws WrongSizeException {
3374 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
3375
3376 final var ba = generateBa();
3377 final var biasX = ba.getElementAtIndex(0);
3378 final var biasY = ba.getElementAtIndex(1);
3379 final var biasZ = ba.getElementAtIndex(2);
3380
3381 final var ma = generateMaCommonAxis();
3382 final var sx = ma.getElementAt(0, 0);
3383 final var sy = ma.getElementAt(1, 1);
3384 final var sz = ma.getElementAt(2, 2);
3385
3386 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3387 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3388 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3389
3390 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bx, by, bz, sx, sy, sz,
3391 this);
3392
3393
3394 assertEquals(biasX, calibrator.getBiasX(), 0.0);
3395 assertEquals(biasY, calibrator.getBiasY(), 0.0);
3396 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3397 final var bx1 = calibrator.getBiasXAsAcceleration();
3398 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3399 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3400 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3401 calibrator.getBiasXAsAcceleration(bx2);
3402 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3403 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3404 final var by1 = calibrator.getBiasYAsAcceleration();
3405 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3406 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3407 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3408 calibrator.getBiasYAsAcceleration(by2);
3409 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3410 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3411 final var bz1 = calibrator.getBiasZAsAcceleration();
3412 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3413 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3414 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3415 calibrator.getBiasZAsAcceleration(bz2);
3416 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3417 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3418 final var biasTriad1 = calibrator.getBiasAsTriad();
3419 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3420 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3421 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3422 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3423 final var biasTriad2 = new AccelerationTriad();
3424 calibrator.getBiasAsTriad(biasTriad2);
3425 assertEquals(biasTriad1, biasTriad2);
3426 assertEquals(sx, calibrator.getInitialSx(), 0.0);
3427 assertEquals(sy, calibrator.getInitialSy(), 0.0);
3428 assertEquals(sz, calibrator.getInitialSz(), 0.0);
3429 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3430 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3431 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3432 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3433 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3434 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3435 final var bias1 = calibrator.getBias();
3436 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3437 final var bias2 = new double[3];
3438 calibrator.getBias(bias2);
3439 assertArrayEquals(bias1, bias2, 0.0);
3440 final var b1 = calibrator.getBiasAsMatrix();
3441 assertEquals(b1, ba);
3442 final var b2 = new Matrix(3, 1);
3443 calibrator.getBiasAsMatrix(b2);
3444 assertEquals(b1, b2);
3445 final var ma1 = calibrator.getInitialMa();
3446 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3447 final var ma2 = new Matrix(3, 3);
3448 calibrator.getInitialMa(ma2);
3449 assertEquals(ma1, ma2);
3450 assertSame(measurements, calibrator.getMeasurements());
3451 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3452 calibrator.getMeasurementType());
3453 assertFalse(calibrator.isOrderedMeasurementsRequired());
3454 assertFalse(calibrator.isQualityScoresRequired());
3455 assertFalse(calibrator.isCommonAxisUsed());
3456 assertSame(this, calibrator.getListener());
3457 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
3458 assertFalse(calibrator.isReady());
3459 assertFalse(calibrator.isRunning());
3460 assertNull(calibrator.getEstimatedMa());
3461 assertNull(calibrator.getEstimatedSx());
3462 assertNull(calibrator.getEstimatedSy());
3463 assertNull(calibrator.getEstimatedSz());
3464 assertNull(calibrator.getEstimatedMxy());
3465 assertNull(calibrator.getEstimatedMxz());
3466 assertNull(calibrator.getEstimatedMyx());
3467 assertNull(calibrator.getEstimatedMyz());
3468 assertNull(calibrator.getEstimatedMzx());
3469 assertNull(calibrator.getEstimatedMzy());
3470 assertNull(calibrator.getEstimatedCovariance());
3471 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3472 assertNull(calibrator.getGroundTruthGravityNorm());
3473 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3474 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3475 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3476 }
3477
3478 @Test
3479 void testConstructor36() throws WrongSizeException {
3480 final var ba = generateBa();
3481 final var biasX = ba.getElementAtIndex(0);
3482 final var biasY = ba.getElementAtIndex(1);
3483 final var biasZ = ba.getElementAtIndex(2);
3484
3485 final var ma = generateMaCommonAxis();
3486 final var sx = ma.getElementAt(0, 0);
3487 final var sy = ma.getElementAt(1, 1);
3488 final var sz = ma.getElementAt(2, 2);
3489
3490 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3491 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3492 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3493
3494 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bx, by, bz,
3495 sx, sy, sz);
3496
3497
3498 assertEquals(biasX, calibrator.getBiasX(), 0.0);
3499 assertEquals(biasY, calibrator.getBiasY(), 0.0);
3500 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3501 final var bx1 = calibrator.getBiasXAsAcceleration();
3502 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3503 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3504 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3505 calibrator.getBiasXAsAcceleration(bx2);
3506 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3507 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3508 final var by1 = calibrator.getBiasYAsAcceleration();
3509 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3510 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3511 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3512 calibrator.getBiasYAsAcceleration(by2);
3513 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3514 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3515 final var bz1 = calibrator.getBiasZAsAcceleration();
3516 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3517 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3518 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3519 calibrator.getBiasZAsAcceleration(bz2);
3520 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3521 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3522 final var biasTriad1 = calibrator.getBiasAsTriad();
3523 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3524 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3525 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3526 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3527 final var biasTriad2 = new AccelerationTriad();
3528 calibrator.getBiasAsTriad(biasTriad2);
3529 assertEquals(biasTriad1, biasTriad2);
3530 assertEquals(sx, calibrator.getInitialSx(), 0.0);
3531 assertEquals(sy, calibrator.getInitialSy(), 0.0);
3532 assertEquals(sz, calibrator.getInitialSz(), 0.0);
3533 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3534 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3535 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3536 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3537 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3538 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3539 final var bias1 = calibrator.getBias();
3540 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3541 final var bias2 = new double[3];
3542 calibrator.getBias(bias2);
3543 assertArrayEquals(bias1, bias2, 0.0);
3544 final var b1 = calibrator.getBiasAsMatrix();
3545 assertEquals(b1, ba);
3546 final var b2 = new Matrix(3, 1);
3547 calibrator.getBiasAsMatrix(b2);
3548 assertEquals(b1, b2);
3549 final var ma1 = calibrator.getInitialMa();
3550 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3551 final var ma2 = new Matrix(3, 3);
3552 calibrator.getInitialMa(ma2);
3553 assertEquals(ma1, ma2);
3554 assertNull(calibrator.getMeasurements());
3555 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3556 calibrator.getMeasurementType());
3557 assertFalse(calibrator.isOrderedMeasurementsRequired());
3558 assertFalse(calibrator.isQualityScoresRequired());
3559 assertTrue(calibrator.isCommonAxisUsed());
3560 assertNull(calibrator.getListener());
3561 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
3562 assertFalse(calibrator.isReady());
3563 assertFalse(calibrator.isRunning());
3564 assertNull(calibrator.getEstimatedMa());
3565 assertNull(calibrator.getEstimatedSx());
3566 assertNull(calibrator.getEstimatedSy());
3567 assertNull(calibrator.getEstimatedSz());
3568 assertNull(calibrator.getEstimatedMxy());
3569 assertNull(calibrator.getEstimatedMxz());
3570 assertNull(calibrator.getEstimatedMyx());
3571 assertNull(calibrator.getEstimatedMyz());
3572 assertNull(calibrator.getEstimatedMzx());
3573 assertNull(calibrator.getEstimatedMzy());
3574 assertNull(calibrator.getEstimatedCovariance());
3575 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3576 assertNull(calibrator.getGroundTruthGravityNorm());
3577 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3578 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3579 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3580 }
3581
3582 @Test
3583 void testConstructor37() throws WrongSizeException {
3584 final var ba = generateBa();
3585 final var biasX = ba.getElementAtIndex(0);
3586 final var biasY = ba.getElementAtIndex(1);
3587 final var biasZ = ba.getElementAtIndex(2);
3588
3589 final var ma = generateMaCommonAxis();
3590 final var sx = ma.getElementAt(0, 0);
3591 final var sy = ma.getElementAt(1, 1);
3592 final var sz = ma.getElementAt(2, 2);
3593
3594 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3595 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3596 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3597
3598 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bx, by, bz,
3599 sx, sy, sz, this);
3600
3601
3602 assertEquals(biasX, calibrator.getBiasX(), 0.0);
3603 assertEquals(biasY, calibrator.getBiasY(), 0.0);
3604 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3605 final var bx1 = calibrator.getBiasXAsAcceleration();
3606 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3607 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3608 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3609 calibrator.getBiasXAsAcceleration(bx2);
3610 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3611 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3612 final var by1 = calibrator.getBiasYAsAcceleration();
3613 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3614 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3615 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3616 calibrator.getBiasYAsAcceleration(by2);
3617 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3618 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3619 final var bz1 = calibrator.getBiasZAsAcceleration();
3620 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3621 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3622 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3623 calibrator.getBiasZAsAcceleration(bz2);
3624 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3625 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3626 final var biasTriad1 = calibrator.getBiasAsTriad();
3627 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3628 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3629 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3630 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3631 final var biasTriad2 = new AccelerationTriad();
3632 calibrator.getBiasAsTriad(biasTriad2);
3633 assertEquals(biasTriad1, biasTriad2);
3634 assertEquals(sx, calibrator.getInitialSx(), 0.0);
3635 assertEquals(sy, calibrator.getInitialSy(), 0.0);
3636 assertEquals(sz, calibrator.getInitialSz(), 0.0);
3637 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3638 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3639 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3640 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3641 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3642 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3643 final var bias1 = calibrator.getBias();
3644 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3645 final var bias2 = new double[3];
3646 calibrator.getBias(bias2);
3647 assertArrayEquals(bias1, bias2, 0.0);
3648 final var b1 = calibrator.getBiasAsMatrix();
3649 assertEquals(b1, ba);
3650 final var b2 = new Matrix(3, 1);
3651 calibrator.getBiasAsMatrix(b2);
3652 assertEquals(b1, b2);
3653 final var ma1 = calibrator.getInitialMa();
3654 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3655 final var ma2 = new Matrix(3, 3);
3656 calibrator.getInitialMa(ma2);
3657 assertEquals(ma1, ma2);
3658 assertNull(calibrator.getMeasurements());
3659 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3660 calibrator.getMeasurementType());
3661 assertFalse(calibrator.isOrderedMeasurementsRequired());
3662 assertFalse(calibrator.isQualityScoresRequired());
3663 assertTrue(calibrator.isCommonAxisUsed());
3664 assertSame(this, calibrator.getListener());
3665 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
3666 assertFalse(calibrator.isReady());
3667 assertFalse(calibrator.isRunning());
3668 assertNull(calibrator.getEstimatedMa());
3669 assertNull(calibrator.getEstimatedSx());
3670 assertNull(calibrator.getEstimatedSy());
3671 assertNull(calibrator.getEstimatedSz());
3672 assertNull(calibrator.getEstimatedMxy());
3673 assertNull(calibrator.getEstimatedMxz());
3674 assertNull(calibrator.getEstimatedMyx());
3675 assertNull(calibrator.getEstimatedMyz());
3676 assertNull(calibrator.getEstimatedMzx());
3677 assertNull(calibrator.getEstimatedMzy());
3678 assertNull(calibrator.getEstimatedCovariance());
3679 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3680 assertNull(calibrator.getGroundTruthGravityNorm());
3681 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3682 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3683 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3684 }
3685
3686 @Test
3687 void testConstructor38() throws WrongSizeException {
3688 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
3689
3690 final var ba = generateBa();
3691 final var biasX = ba.getElementAtIndex(0);
3692 final var biasY = ba.getElementAtIndex(1);
3693 final var biasZ = ba.getElementAtIndex(2);
3694
3695 final var ma = generateMaCommonAxis();
3696 final var sx = ma.getElementAt(0, 0);
3697 final var sy = ma.getElementAt(1, 1);
3698 final var sz = ma.getElementAt(2, 2);
3699
3700 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3701 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3702 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3703
3704 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
3705 bx, by, bz, sx, sy, sz);
3706
3707
3708 assertEquals(biasX, calibrator.getBiasX(), 0.0);
3709 assertEquals(biasY, calibrator.getBiasY(), 0.0);
3710 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3711 final var bx1 = calibrator.getBiasXAsAcceleration();
3712 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3713 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3714 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3715 calibrator.getBiasXAsAcceleration(bx2);
3716 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3717 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3718 final var by1 = calibrator.getBiasYAsAcceleration();
3719 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3720 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3721 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3722 calibrator.getBiasYAsAcceleration(by2);
3723 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3724 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3725 final var bz1 = calibrator.getBiasZAsAcceleration();
3726 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3727 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3728 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3729 calibrator.getBiasZAsAcceleration(bz2);
3730 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3731 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3732 final var biasTriad1 = calibrator.getBiasAsTriad();
3733 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3734 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3735 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3736 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3737 final var biasTriad2 = new AccelerationTriad();
3738 calibrator.getBiasAsTriad(biasTriad2);
3739 assertEquals(biasTriad1, biasTriad2);
3740 assertEquals(sx, calibrator.getInitialSx(), 0.0);
3741 assertEquals(sy, calibrator.getInitialSy(), 0.0);
3742 assertEquals(sz, calibrator.getInitialSz(), 0.0);
3743 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3744 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3745 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3746 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3747 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3748 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3749 final var bias1 = calibrator.getBias();
3750 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3751 final var bias2 = new double[3];
3752 calibrator.getBias(bias2);
3753 assertArrayEquals(bias1, bias2, 0.0);
3754 final var b1 = calibrator.getBiasAsMatrix();
3755 assertEquals(b1, ba);
3756 final var b2 = new Matrix(3, 1);
3757 calibrator.getBiasAsMatrix(b2);
3758 assertEquals(b1, b2);
3759 final var ma1 = calibrator.getInitialMa();
3760 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3761 final var ma2 = new Matrix(3, 3);
3762 calibrator.getInitialMa(ma2);
3763 assertEquals(ma1, ma2);
3764 assertSame(measurements, calibrator.getMeasurements());
3765 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3766 calibrator.getMeasurementType());
3767 assertFalse(calibrator.isOrderedMeasurementsRequired());
3768 assertFalse(calibrator.isQualityScoresRequired());
3769 assertTrue(calibrator.isCommonAxisUsed());
3770 assertNull(calibrator.getListener());
3771 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
3772 assertFalse(calibrator.isReady());
3773 assertFalse(calibrator.isRunning());
3774 assertNull(calibrator.getEstimatedMa());
3775 assertNull(calibrator.getEstimatedSx());
3776 assertNull(calibrator.getEstimatedSy());
3777 assertNull(calibrator.getEstimatedSz());
3778 assertNull(calibrator.getEstimatedMxy());
3779 assertNull(calibrator.getEstimatedMxz());
3780 assertNull(calibrator.getEstimatedMyx());
3781 assertNull(calibrator.getEstimatedMyz());
3782 assertNull(calibrator.getEstimatedMzx());
3783 assertNull(calibrator.getEstimatedMzy());
3784 assertNull(calibrator.getEstimatedCovariance());
3785 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3786 assertNull(calibrator.getGroundTruthGravityNorm());
3787 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3788 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3789 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3790 }
3791
3792 @Test
3793 void testConstructor39() throws WrongSizeException {
3794 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
3795
3796 final var ba = generateBa();
3797 final var biasX = ba.getElementAtIndex(0);
3798 final var biasY = ba.getElementAtIndex(1);
3799 final var biasZ = ba.getElementAtIndex(2);
3800
3801 final var ma = generateMaCommonAxis();
3802 final var sx = ma.getElementAt(0, 0);
3803 final var sy = ma.getElementAt(1, 1);
3804 final var sz = ma.getElementAt(2, 2);
3805
3806 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3807 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3808 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3809
3810 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
3811 bx, by, bz, sx, sy, sz, this);
3812
3813
3814 assertEquals(biasX, calibrator.getBiasX(), 0.0);
3815 assertEquals(biasY, calibrator.getBiasY(), 0.0);
3816 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3817 final var bx1 = calibrator.getBiasXAsAcceleration();
3818 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3819 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3820 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3821 calibrator.getBiasXAsAcceleration(bx2);
3822 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3823 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3824 final var by1 = calibrator.getBiasYAsAcceleration();
3825 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3826 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3827 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3828 calibrator.getBiasYAsAcceleration(by2);
3829 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3830 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3831 final var bz1 = calibrator.getBiasZAsAcceleration();
3832 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3833 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3834 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3835 calibrator.getBiasZAsAcceleration(bz2);
3836 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3837 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3838 final var biasTriad1 = calibrator.getBiasAsTriad();
3839 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3840 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3841 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3842 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3843 final var biasTriad2 = new AccelerationTriad();
3844 calibrator.getBiasAsTriad(biasTriad2);
3845 assertEquals(biasTriad1, biasTriad2);
3846 assertEquals(sx, calibrator.getInitialSx(), 0.0);
3847 assertEquals(sy, calibrator.getInitialSy(), 0.0);
3848 assertEquals(sz, calibrator.getInitialSz(), 0.0);
3849 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3850 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3851 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3852 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3853 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3854 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3855 final var bias1 = calibrator.getBias();
3856 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3857 final var bias2 = new double[3];
3858 calibrator.getBias(bias2);
3859 assertArrayEquals(bias1, bias2, 0.0);
3860 final var b1 = calibrator.getBiasAsMatrix();
3861 assertEquals(b1, ba);
3862 final var b2 = new Matrix(3, 1);
3863 calibrator.getBiasAsMatrix(b2);
3864 assertEquals(b1, b2);
3865 final var ma1 = calibrator.getInitialMa();
3866 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3867 final var ma2 = new Matrix(3, 3);
3868 calibrator.getInitialMa(ma2);
3869 assertEquals(ma1, ma2);
3870 assertSame(measurements, calibrator.getMeasurements());
3871 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3872 calibrator.getMeasurementType());
3873 assertFalse(calibrator.isOrderedMeasurementsRequired());
3874 assertFalse(calibrator.isQualityScoresRequired());
3875 assertTrue(calibrator.isCommonAxisUsed());
3876 assertSame(this, calibrator.getListener());
3877 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
3878 assertFalse(calibrator.isReady());
3879 assertFalse(calibrator.isRunning());
3880 assertNull(calibrator.getEstimatedMa());
3881 assertNull(calibrator.getEstimatedSx());
3882 assertNull(calibrator.getEstimatedSy());
3883 assertNull(calibrator.getEstimatedSz());
3884 assertNull(calibrator.getEstimatedMxy());
3885 assertNull(calibrator.getEstimatedMxz());
3886 assertNull(calibrator.getEstimatedMyx());
3887 assertNull(calibrator.getEstimatedMyz());
3888 assertNull(calibrator.getEstimatedMzx());
3889 assertNull(calibrator.getEstimatedMzy());
3890 assertNull(calibrator.getEstimatedCovariance());
3891 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3892 assertNull(calibrator.getGroundTruthGravityNorm());
3893 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3894 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3895 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3896 }
3897
3898 @Test
3899 void testConstructor40() throws WrongSizeException {
3900 final var ba = generateBa();
3901 final var biasX = ba.getElementAtIndex(0);
3902 final var biasY = ba.getElementAtIndex(1);
3903 final var biasZ = ba.getElementAtIndex(2);
3904
3905 final var ma = generateMaCommonAxis();
3906 final var sx = ma.getElementAt(0, 0);
3907 final var sy = ma.getElementAt(1, 1);
3908 final var sz = ma.getElementAt(2, 2);
3909 final var mxy = ma.getElementAt(0, 1);
3910 final var mxz = ma.getElementAt(0, 2);
3911 final var myx = ma.getElementAt(1, 0);
3912 final var myz = ma.getElementAt(1, 2);
3913 final var mzx = ma.getElementAt(2, 0);
3914 final var mzy = ma.getElementAt(2, 1);
3915
3916 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(biasX, biasY, biasZ, sx, sy, sz,
3917 mxy, mxz, myx, myz, mzx, mzy);
3918
3919
3920 assertEquals(biasX, calibrator.getBiasX(), 0.0);
3921 assertEquals(biasY, calibrator.getBiasY(), 0.0);
3922 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3923 final var bx1 = calibrator.getBiasXAsAcceleration();
3924 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3925 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3926 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3927 calibrator.getBiasXAsAcceleration(bx2);
3928 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3929 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3930 final var by1 = calibrator.getBiasYAsAcceleration();
3931 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3932 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3933 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3934 calibrator.getBiasYAsAcceleration(by2);
3935 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3936 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3937 final var bz1 = calibrator.getBiasZAsAcceleration();
3938 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3939 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3940 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3941 calibrator.getBiasZAsAcceleration(bz2);
3942 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3943 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3944 final var biasTriad1 = calibrator.getBiasAsTriad();
3945 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3946 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3947 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3948 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3949 final var biasTriad2 = new AccelerationTriad();
3950 calibrator.getBiasAsTriad(biasTriad2);
3951 assertEquals(biasTriad1, biasTriad2);
3952 assertEquals(sx, calibrator.getInitialSx(), 0.0);
3953 assertEquals(sy, calibrator.getInitialSy(), 0.0);
3954 assertEquals(sz, calibrator.getInitialSz(), 0.0);
3955 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
3956 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
3957 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
3958 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
3959 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
3960 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
3961 final var bias1 = calibrator.getBias();
3962 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3963 final var bias2 = new double[3];
3964 calibrator.getBias(bias2);
3965 assertArrayEquals(bias1, bias2, 0.0);
3966 final var b1 = calibrator.getBiasAsMatrix();
3967 assertEquals(b1, ba);
3968 final var b2 = new Matrix(3, 1);
3969 calibrator.getBiasAsMatrix(b2);
3970 assertEquals(b1, b2);
3971 final var ma1 = new Matrix(3, 3);
3972 ma1.setSubmatrix(0, 0, 2, 2,
3973 new double[]{sx, myx, mzx,
3974 mxy, sy, mzy,
3975 mxz, myz, sz});
3976 assertEquals(calibrator.getInitialMa(), ma1);
3977 final var ma2 = new Matrix(3, 3);
3978 calibrator.getInitialMa(ma2);
3979 assertEquals(ma1, ma2);
3980 assertNull(calibrator.getMeasurements());
3981 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3982 calibrator.getMeasurementType());
3983 assertFalse(calibrator.isOrderedMeasurementsRequired());
3984 assertFalse(calibrator.isQualityScoresRequired());
3985 assertFalse(calibrator.isCommonAxisUsed());
3986 assertNull(calibrator.getListener());
3987 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
3988 assertFalse(calibrator.isReady());
3989 assertFalse(calibrator.isRunning());
3990 assertNull(calibrator.getEstimatedMa());
3991 assertNull(calibrator.getEstimatedSx());
3992 assertNull(calibrator.getEstimatedSy());
3993 assertNull(calibrator.getEstimatedSz());
3994 assertNull(calibrator.getEstimatedMxy());
3995 assertNull(calibrator.getEstimatedMxz());
3996 assertNull(calibrator.getEstimatedMyx());
3997 assertNull(calibrator.getEstimatedMyz());
3998 assertNull(calibrator.getEstimatedMzx());
3999 assertNull(calibrator.getEstimatedMzy());
4000 assertNull(calibrator.getEstimatedCovariance());
4001 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4002 assertNull(calibrator.getGroundTruthGravityNorm());
4003 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4004 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4005 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4006 }
4007
4008 @Test
4009 void testConstructor41() throws WrongSizeException {
4010 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
4011
4012 final var ba = generateBa();
4013 final var biasX = ba.getElementAtIndex(0);
4014 final var biasY = ba.getElementAtIndex(1);
4015 final var biasZ = ba.getElementAtIndex(2);
4016
4017 final var ma = generateMaCommonAxis();
4018 final var sx = ma.getElementAt(0, 0);
4019 final var sy = ma.getElementAt(1, 1);
4020 final var sz = ma.getElementAt(2, 2);
4021 final var mxy = ma.getElementAt(0, 1);
4022 final var mxz = ma.getElementAt(0, 2);
4023 final var myx = ma.getElementAt(1, 0);
4024 final var myz = ma.getElementAt(1, 2);
4025 final var mzx = ma.getElementAt(2, 0);
4026 final var mzy = ma.getElementAt(2, 1);
4027
4028 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
4029 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4030
4031
4032 assertEquals(biasX, calibrator.getBiasX(), 0.0);
4033 assertEquals(biasY, calibrator.getBiasY(), 0.0);
4034 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4035 final var bx1 = calibrator.getBiasXAsAcceleration();
4036 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4037 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4038 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4039 calibrator.getBiasXAsAcceleration(bx2);
4040 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4041 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4042 final var by1 = calibrator.getBiasYAsAcceleration();
4043 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4044 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4045 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4046 calibrator.getBiasYAsAcceleration(by2);
4047 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4048 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4049 final var bz1 = calibrator.getBiasZAsAcceleration();
4050 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4051 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4052 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4053 calibrator.getBiasZAsAcceleration(bz2);
4054 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4055 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4056 final var biasTriad1 = calibrator.getBiasAsTriad();
4057 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4058 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4059 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4060 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4061 final var biasTriad2 = new AccelerationTriad();
4062 calibrator.getBiasAsTriad(biasTriad2);
4063 assertEquals(biasTriad1, biasTriad2);
4064 assertEquals(sx, calibrator.getInitialSx(), 0.0);
4065 assertEquals(sy, calibrator.getInitialSy(), 0.0);
4066 assertEquals(sz, calibrator.getInitialSz(), 0.0);
4067 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4068 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4069 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4070 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4071 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4072 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4073 final var bias1 = calibrator.getBias();
4074 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4075 final var bias2 = new double[3];
4076 calibrator.getBias(bias2);
4077 assertArrayEquals(bias1, bias2, 0.0);
4078 final var b1 = calibrator.getBiasAsMatrix();
4079 assertEquals(b1, ba);
4080 final var b2 = new Matrix(3, 1);
4081 calibrator.getBiasAsMatrix(b2);
4082 assertEquals(b1, b2);
4083 final var ma1 = new Matrix(3, 3);
4084 ma1.setSubmatrix(0, 0, 2, 2,
4085 new double[]{sx, myx, mzx,
4086 mxy, sy, mzy,
4087 mxz, myz, sz});
4088 assertEquals(ma1, calibrator.getInitialMa());
4089 final var ma2 = new Matrix(3, 3);
4090 calibrator.getInitialMa(ma2);
4091 assertEquals(ma1, ma2);
4092 assertSame(measurements, calibrator.getMeasurements());
4093 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4094 calibrator.getMeasurementType());
4095 assertFalse(calibrator.isOrderedMeasurementsRequired());
4096 assertFalse(calibrator.isQualityScoresRequired());
4097 assertFalse(calibrator.isCommonAxisUsed());
4098 assertNull(calibrator.getListener());
4099 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
4100 assertFalse(calibrator.isReady());
4101 assertFalse(calibrator.isRunning());
4102 assertNull(calibrator.getEstimatedMa());
4103 assertNull(calibrator.getEstimatedSx());
4104 assertNull(calibrator.getEstimatedSy());
4105 assertNull(calibrator.getEstimatedSz());
4106 assertNull(calibrator.getEstimatedMxy());
4107 assertNull(calibrator.getEstimatedMxz());
4108 assertNull(calibrator.getEstimatedMyx());
4109 assertNull(calibrator.getEstimatedMyz());
4110 assertNull(calibrator.getEstimatedMzx());
4111 assertNull(calibrator.getEstimatedMzy());
4112 assertNull(calibrator.getEstimatedCovariance());
4113 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4114 assertNull(calibrator.getGroundTruthGravityNorm());
4115 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4116 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4117 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4118 }
4119
4120 @Test
4121 void testConstructor42() throws WrongSizeException {
4122 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
4123
4124 final var ba = generateBa();
4125 final var biasX = ba.getElementAtIndex(0);
4126 final var biasY = ba.getElementAtIndex(1);
4127 final var biasZ = ba.getElementAtIndex(2);
4128
4129 final var ma = generateMaCommonAxis();
4130 final var sx = ma.getElementAt(0, 0);
4131 final var sy = ma.getElementAt(1, 1);
4132 final var sz = ma.getElementAt(2, 2);
4133 final var mxy = ma.getElementAt(0, 1);
4134 final var mxz = ma.getElementAt(0, 2);
4135 final var myx = ma.getElementAt(1, 0);
4136 final var myz = ma.getElementAt(1, 2);
4137 final var mzx = ma.getElementAt(2, 0);
4138 final var mzy = ma.getElementAt(2, 1);
4139
4140 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
4141 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4142
4143
4144 assertEquals(biasX, calibrator.getBiasX(), 0.0);
4145 assertEquals(biasY, calibrator.getBiasY(), 0.0);
4146 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4147 final var bx1 = calibrator.getBiasXAsAcceleration();
4148 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4149 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4150 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4151 calibrator.getBiasXAsAcceleration(bx2);
4152 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4153 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4154 final var by1 = calibrator.getBiasYAsAcceleration();
4155 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4156 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4157 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4158 calibrator.getBiasYAsAcceleration(by2);
4159 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4160 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4161 final var bz1 = calibrator.getBiasZAsAcceleration();
4162 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4163 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4164 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4165 calibrator.getBiasZAsAcceleration(bz2);
4166 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4167 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4168 final var biasTriad1 = calibrator.getBiasAsTriad();
4169 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4170 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4171 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4172 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4173 final var biasTriad2 = new AccelerationTriad();
4174 calibrator.getBiasAsTriad(biasTriad2);
4175 assertEquals(biasTriad1, biasTriad2);
4176 assertEquals(sx, calibrator.getInitialSx(), 0.0);
4177 assertEquals(sy, calibrator.getInitialSy(), 0.0);
4178 assertEquals(sz, calibrator.getInitialSz(), 0.0);
4179 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4180 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4181 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4182 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4183 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4184 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4185 final var bias1 = calibrator.getBias();
4186 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4187 final var bias2 = new double[3];
4188 calibrator.getBias(bias2);
4189 assertArrayEquals(bias1, bias2, 0.0);
4190 final var b1 = calibrator.getBiasAsMatrix();
4191 assertEquals(b1, ba);
4192 final var b2 = new Matrix(3, 1);
4193 calibrator.getBiasAsMatrix(b2);
4194 assertEquals(b1, b2);
4195 final var ma1 = new Matrix(3, 3);
4196 ma1.setSubmatrix(0, 0, 2, 2,
4197 new double[]{sx, myx, mzx,
4198 mxy, sy, mzy,
4199 mxz, myz, sz});
4200 assertEquals(calibrator.getInitialMa(), ma1);
4201 final var ma2 = new Matrix(3, 3);
4202 calibrator.getInitialMa(ma2);
4203 assertEquals(ma1, ma2);
4204 assertSame(calibrator.getMeasurements(), measurements);
4205 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4206 calibrator.getMeasurementType());
4207 assertFalse(calibrator.isOrderedMeasurementsRequired());
4208 assertFalse(calibrator.isQualityScoresRequired());
4209 assertFalse(calibrator.isCommonAxisUsed());
4210 assertSame(this, calibrator.getListener());
4211 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
4212 assertFalse(calibrator.isReady());
4213 assertFalse(calibrator.isRunning());
4214 assertNull(calibrator.getEstimatedMa());
4215 assertNull(calibrator.getEstimatedSx());
4216 assertNull(calibrator.getEstimatedSy());
4217 assertNull(calibrator.getEstimatedSz());
4218 assertNull(calibrator.getEstimatedMxy());
4219 assertNull(calibrator.getEstimatedMxz());
4220 assertNull(calibrator.getEstimatedMyx());
4221 assertNull(calibrator.getEstimatedMyz());
4222 assertNull(calibrator.getEstimatedMzx());
4223 assertNull(calibrator.getEstimatedMzy());
4224 assertNull(calibrator.getEstimatedCovariance());
4225 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4226 assertNull(calibrator.getGroundTruthGravityNorm());
4227 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4228 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4229 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4230 }
4231
4232 @Test
4233 void testConstructor43() throws WrongSizeException {
4234 final var ba = generateBa();
4235 final var biasX = ba.getElementAtIndex(0);
4236 final var biasY = ba.getElementAtIndex(1);
4237 final var biasZ = ba.getElementAtIndex(2);
4238
4239 final var ma = generateMaCommonAxis();
4240 final var sx = ma.getElementAt(0, 0);
4241 final var sy = ma.getElementAt(1, 1);
4242 final var sz = ma.getElementAt(2, 2);
4243 final var mxy = ma.getElementAt(0, 1);
4244 final var mxz = ma.getElementAt(0, 2);
4245 final var myx = ma.getElementAt(1, 0);
4246 final var myz = ma.getElementAt(1, 2);
4247 final var mzx = ma.getElementAt(2, 0);
4248 final var mzy = ma.getElementAt(2, 1);
4249
4250 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true,
4251 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4252
4253
4254 assertEquals(biasX, calibrator.getBiasX(), 0.0);
4255 assertEquals(biasY, calibrator.getBiasY(), 0.0);
4256 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4257 final var bx1 = calibrator.getBiasXAsAcceleration();
4258 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4259 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4260 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4261 calibrator.getBiasXAsAcceleration(bx2);
4262 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4263 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4264 final var by1 = calibrator.getBiasYAsAcceleration();
4265 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4266 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4267 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4268 calibrator.getBiasYAsAcceleration(by2);
4269 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4270 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4271 final var bz1 = calibrator.getBiasZAsAcceleration();
4272 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4273 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4274 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4275 calibrator.getBiasZAsAcceleration(bz2);
4276 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4277 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4278 final var biasTriad1 = calibrator.getBiasAsTriad();
4279 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4280 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4281 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4282 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4283 final var biasTriad2 = new AccelerationTriad();
4284 calibrator.getBiasAsTriad(biasTriad2);
4285 assertEquals(biasTriad1, biasTriad2);
4286 assertEquals(sx, calibrator.getInitialSx(), 0.0);
4287 assertEquals(sy, calibrator.getInitialSy(), 0.0);
4288 assertEquals(sz, calibrator.getInitialSz(), 0.0);
4289 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4290 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4291 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4292 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4293 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4294 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4295 final var bias1 = calibrator.getBias();
4296 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4297 final var bias2 = new double[3];
4298 calibrator.getBias(bias2);
4299 assertArrayEquals(bias1, bias2, 0.0);
4300 final var b1 = calibrator.getBiasAsMatrix();
4301 assertEquals(b1, ba);
4302 final var b2 = new Matrix(3, 1);
4303 calibrator.getBiasAsMatrix(b2);
4304 assertEquals(b1, b2);
4305 final var ma1 = new Matrix(3, 3);
4306 ma1.setSubmatrix(0, 0, 2, 2,
4307 new double[]{sx, myx, mzx,
4308 mxy, sy, mzy,
4309 mxz, myz, sz});
4310 assertEquals(ma1, calibrator.getInitialMa());
4311 final var ma2 = new Matrix(3, 3);
4312 calibrator.getInitialMa(ma2);
4313 assertEquals(ma1, ma2);
4314 assertNull(calibrator.getMeasurements());
4315 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4316 calibrator.getMeasurementType());
4317 assertFalse(calibrator.isOrderedMeasurementsRequired());
4318 assertFalse(calibrator.isQualityScoresRequired());
4319 assertTrue(calibrator.isCommonAxisUsed());
4320 assertNull(calibrator.getListener());
4321 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
4322 assertFalse(calibrator.isReady());
4323 assertFalse(calibrator.isRunning());
4324 assertNull(calibrator.getEstimatedMa());
4325 assertNull(calibrator.getEstimatedSx());
4326 assertNull(calibrator.getEstimatedSy());
4327 assertNull(calibrator.getEstimatedSz());
4328 assertNull(calibrator.getEstimatedMxy());
4329 assertNull(calibrator.getEstimatedMxz());
4330 assertNull(calibrator.getEstimatedMyx());
4331 assertNull(calibrator.getEstimatedMyz());
4332 assertNull(calibrator.getEstimatedMzx());
4333 assertNull(calibrator.getEstimatedMzy());
4334 assertNull(calibrator.getEstimatedCovariance());
4335 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4336 assertNull(calibrator.getGroundTruthGravityNorm());
4337 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4338 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4339 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4340 }
4341
4342 @Test
4343 void testConstructor44() throws WrongSizeException {
4344 final var ba = generateBa();
4345 final var biasX = ba.getElementAtIndex(0);
4346 final var biasY = ba.getElementAtIndex(1);
4347 final var biasZ = ba.getElementAtIndex(2);
4348
4349 final var ma = generateMaCommonAxis();
4350 final var sx = ma.getElementAt(0, 0);
4351 final var sy = ma.getElementAt(1, 1);
4352 final var sz = ma.getElementAt(2, 2);
4353 final var mxy = ma.getElementAt(0, 1);
4354 final var mxz = ma.getElementAt(0, 2);
4355 final var myx = ma.getElementAt(1, 0);
4356 final var myz = ma.getElementAt(1, 2);
4357 final var mzx = ma.getElementAt(2, 0);
4358 final var mzy = ma.getElementAt(2, 1);
4359
4360 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true,
4361 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4362
4363
4364 assertEquals(biasX, calibrator.getBiasX(), 0.0);
4365 assertEquals(biasY, calibrator.getBiasY(), 0.0);
4366 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4367 final var bx1 = calibrator.getBiasXAsAcceleration();
4368 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4369 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4370 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4371 calibrator.getBiasXAsAcceleration(bx2);
4372 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4373 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4374 final var by1 = calibrator.getBiasYAsAcceleration();
4375 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4376 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4377 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4378 calibrator.getBiasYAsAcceleration(by2);
4379 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4380 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4381 final var bz1 = calibrator.getBiasZAsAcceleration();
4382 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4383 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4384 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4385 calibrator.getBiasZAsAcceleration(bz2);
4386 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4387 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4388 final var biasTriad1 = calibrator.getBiasAsTriad();
4389 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4390 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4391 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4392 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4393 final var biasTriad2 = new AccelerationTriad();
4394 calibrator.getBiasAsTriad(biasTriad2);
4395 assertEquals(biasTriad1, biasTriad2);
4396 assertEquals(sx, calibrator.getInitialSx(), 0.0);
4397 assertEquals(sy, calibrator.getInitialSy(), 0.0);
4398 assertEquals(sz, calibrator.getInitialSz(), 0.0);
4399 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4400 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4401 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4402 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4403 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4404 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4405 final var bias1 = calibrator.getBias();
4406 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4407 final var bias2 = new double[3];
4408 calibrator.getBias(bias2);
4409 assertArrayEquals(bias1, bias2, 0.0);
4410 final var b1 = calibrator.getBiasAsMatrix();
4411 assertEquals(b1, ba);
4412 final var b2 = new Matrix(3, 1);
4413 calibrator.getBiasAsMatrix(b2);
4414 assertEquals(b1, b2);
4415 final var ma1 = new Matrix(3, 3);
4416 ma1.setSubmatrix(0, 0, 2, 2,
4417 new double[]{sx, myx, mzx,
4418 mxy, sy, mzy,
4419 mxz, myz, sz});
4420 assertEquals(ma1, calibrator.getInitialMa());
4421 final var ma2 = new Matrix(3, 3);
4422 calibrator.getInitialMa(ma2);
4423 assertEquals(ma1, ma2);
4424 assertNull(calibrator.getMeasurements());
4425 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4426 calibrator.getMeasurementType());
4427 assertFalse(calibrator.isOrderedMeasurementsRequired());
4428 assertFalse(calibrator.isQualityScoresRequired());
4429 assertTrue(calibrator.isCommonAxisUsed());
4430 assertSame(this, calibrator.getListener());
4431 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
4432 assertFalse(calibrator.isReady());
4433 assertFalse(calibrator.isRunning());
4434 assertNull(calibrator.getEstimatedMa());
4435 assertNull(calibrator.getEstimatedSx());
4436 assertNull(calibrator.getEstimatedSy());
4437 assertNull(calibrator.getEstimatedSz());
4438 assertNull(calibrator.getEstimatedMxy());
4439 assertNull(calibrator.getEstimatedMxz());
4440 assertNull(calibrator.getEstimatedMyx());
4441 assertNull(calibrator.getEstimatedMyz());
4442 assertNull(calibrator.getEstimatedMzx());
4443 assertNull(calibrator.getEstimatedMzy());
4444 assertNull(calibrator.getEstimatedCovariance());
4445 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4446 assertNull(calibrator.getGroundTruthGravityNorm());
4447 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4448 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4449 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4450 }
4451
4452 @Test
4453 void testConstructor45() throws WrongSizeException {
4454 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
4455
4456 final var ba = generateBa();
4457 final var biasX = ba.getElementAtIndex(0);
4458 final var biasY = ba.getElementAtIndex(1);
4459 final var biasZ = ba.getElementAtIndex(2);
4460
4461 final var ma = generateMaCommonAxis();
4462 final var sx = ma.getElementAt(0, 0);
4463 final var sy = ma.getElementAt(1, 1);
4464 final var sz = ma.getElementAt(2, 2);
4465 final var mxy = ma.getElementAt(0, 1);
4466 final var mxz = ma.getElementAt(0, 2);
4467 final var myx = ma.getElementAt(1, 0);
4468 final var myz = ma.getElementAt(1, 2);
4469 final var mzx = ma.getElementAt(2, 0);
4470 final var mzy = ma.getElementAt(2, 1);
4471
4472 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
4473 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4474
4475
4476 assertEquals(biasX, calibrator.getBiasX(), 0.0);
4477 assertEquals(biasY, calibrator.getBiasY(), 0.0);
4478 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4479 final var bx1 = calibrator.getBiasXAsAcceleration();
4480 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4481 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4482 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4483 calibrator.getBiasXAsAcceleration(bx2);
4484 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4485 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4486 final var by1 = calibrator.getBiasYAsAcceleration();
4487 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4488 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4489 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4490 calibrator.getBiasYAsAcceleration(by2);
4491 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4492 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4493 final var bz1 = calibrator.getBiasZAsAcceleration();
4494 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4495 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4496 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4497 calibrator.getBiasZAsAcceleration(bz2);
4498 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4499 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4500 final var biasTriad1 = calibrator.getBiasAsTriad();
4501 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4502 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4503 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4504 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4505 final var biasTriad2 = new AccelerationTriad();
4506 calibrator.getBiasAsTriad(biasTriad2);
4507 assertEquals(biasTriad1, biasTriad2);
4508 assertEquals(sx, calibrator.getInitialSx(), 0.0);
4509 assertEquals(sy, calibrator.getInitialSy(), 0.0);
4510 assertEquals(sz, calibrator.getInitialSz(), 0.0);
4511 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4512 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4513 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4514 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4515 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4516 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4517 final var bias1 = calibrator.getBias();
4518 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4519 final var bias2 = new double[3];
4520 calibrator.getBias(bias2);
4521 assertArrayEquals(bias1, bias2, 0.0);
4522 final var b1 = calibrator.getBiasAsMatrix();
4523 assertEquals(b1, ba);
4524 final var b2 = new Matrix(3, 1);
4525 calibrator.getBiasAsMatrix(b2);
4526 assertEquals(b1, b2);
4527 final var ma1 = new Matrix(3, 3);
4528 ma1.setSubmatrix(0, 0, 2, 2,
4529 new double[]{sx, myx, mzx,
4530 mxy, sy, mzy,
4531 mxz, myz, sz});
4532 assertEquals(ma1, calibrator.getInitialMa());
4533 final var ma2 = new Matrix(3, 3);
4534 calibrator.getInitialMa(ma2);
4535 assertEquals(ma1, ma2);
4536 assertSame(measurements, calibrator.getMeasurements());
4537 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4538 calibrator.getMeasurementType());
4539 assertFalse(calibrator.isOrderedMeasurementsRequired());
4540 assertFalse(calibrator.isQualityScoresRequired());
4541 assertTrue(calibrator.isCommonAxisUsed());
4542 assertNull(calibrator.getListener());
4543 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
4544 assertFalse(calibrator.isReady());
4545 assertFalse(calibrator.isRunning());
4546 assertNull(calibrator.getEstimatedMa());
4547 assertNull(calibrator.getEstimatedSx());
4548 assertNull(calibrator.getEstimatedSy());
4549 assertNull(calibrator.getEstimatedSz());
4550 assertNull(calibrator.getEstimatedMxy());
4551 assertNull(calibrator.getEstimatedMxz());
4552 assertNull(calibrator.getEstimatedMyx());
4553 assertNull(calibrator.getEstimatedMyz());
4554 assertNull(calibrator.getEstimatedMzx());
4555 assertNull(calibrator.getEstimatedMzy());
4556 assertNull(calibrator.getEstimatedCovariance());
4557 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4558 assertNull(calibrator.getGroundTruthGravityNorm());
4559 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4560 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4561 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4562 }
4563
4564 @Test
4565 void testConstructor46() throws WrongSizeException {
4566 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
4567
4568 final var ba = generateBa();
4569 final var biasX = ba.getElementAtIndex(0);
4570 final var biasY = ba.getElementAtIndex(1);
4571 final var biasZ = ba.getElementAtIndex(2);
4572
4573 final var ma = generateMaCommonAxis();
4574 final var sx = ma.getElementAt(0, 0);
4575 final var sy = ma.getElementAt(1, 1);
4576 final var sz = ma.getElementAt(2, 2);
4577 final var mxy = ma.getElementAt(0, 1);
4578 final var mxz = ma.getElementAt(0, 2);
4579 final var myx = ma.getElementAt(1, 0);
4580 final var myz = ma.getElementAt(1, 2);
4581 final var mzx = ma.getElementAt(2, 0);
4582 final var mzy = ma.getElementAt(2, 1);
4583
4584 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
4585 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4586
4587
4588 assertEquals(biasX, calibrator.getBiasX(), 0.0);
4589 assertEquals(biasY, calibrator.getBiasY(), 0.0);
4590 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4591 final var bx1 = calibrator.getBiasXAsAcceleration();
4592 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4593 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4594 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4595 calibrator.getBiasXAsAcceleration(bx2);
4596 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4597 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4598 final var by1 = calibrator.getBiasYAsAcceleration();
4599 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4600 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4601 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4602 calibrator.getBiasYAsAcceleration(by2);
4603 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4604 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4605 final var bz1 = calibrator.getBiasZAsAcceleration();
4606 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4607 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4608 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4609 calibrator.getBiasZAsAcceleration(bz2);
4610 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4611 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4612 final var biasTriad1 = calibrator.getBiasAsTriad();
4613 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4614 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4615 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4616 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4617 final var biasTriad2 = new AccelerationTriad();
4618 calibrator.getBiasAsTriad(biasTriad2);
4619 assertEquals(biasTriad1, biasTriad2);
4620 assertEquals(sx, calibrator.getInitialSx(), 0.0);
4621 assertEquals(sy, calibrator.getInitialSy(), 0.0);
4622 assertEquals(sz, calibrator.getInitialSz(), 0.0);
4623 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4624 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4625 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4626 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4627 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4628 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4629 final var bias1 = calibrator.getBias();
4630 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4631 final var bias2 = new double[3];
4632 calibrator.getBias(bias2);
4633 assertArrayEquals(bias1, bias2, 0.0);
4634 final var b1 = calibrator.getBiasAsMatrix();
4635 assertEquals(b1, ba);
4636 final var b2 = new Matrix(3, 1);
4637 calibrator.getBiasAsMatrix(b2);
4638 assertEquals(b1, b2);
4639 final var ma1 = new Matrix(3, 3);
4640 ma1.setSubmatrix(0, 0, 2, 2,
4641 new double[]{sx, myx, mzx,
4642 mxy, sy, mzy,
4643 mxz, myz, sz});
4644 assertEquals(ma1, calibrator.getInitialMa());
4645 final var ma2 = new Matrix(3, 3);
4646 calibrator.getInitialMa(ma2);
4647 assertEquals(ma1, ma2);
4648 assertSame(measurements, calibrator.getMeasurements());
4649 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4650 calibrator.getMeasurementType());
4651 assertFalse(calibrator.isOrderedMeasurementsRequired());
4652 assertFalse(calibrator.isQualityScoresRequired());
4653 assertTrue(calibrator.isCommonAxisUsed());
4654 assertSame(this, calibrator.getListener());
4655 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
4656 assertFalse(calibrator.isReady());
4657 assertFalse(calibrator.isRunning());
4658 assertNull(calibrator.getEstimatedMa());
4659 assertNull(calibrator.getEstimatedSx());
4660 assertNull(calibrator.getEstimatedSy());
4661 assertNull(calibrator.getEstimatedSz());
4662 assertNull(calibrator.getEstimatedMxy());
4663 assertNull(calibrator.getEstimatedMxz());
4664 assertNull(calibrator.getEstimatedMyx());
4665 assertNull(calibrator.getEstimatedMyz());
4666 assertNull(calibrator.getEstimatedMzx());
4667 assertNull(calibrator.getEstimatedMzy());
4668 assertNull(calibrator.getEstimatedCovariance());
4669 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4670 assertNull(calibrator.getGroundTruthGravityNorm());
4671 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4672 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4673 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4674 }
4675
4676 @Test
4677 void testConstructor47() throws WrongSizeException {
4678 final var ba = generateBa();
4679 final var biasX = ba.getElementAtIndex(0);
4680 final var biasY = ba.getElementAtIndex(1);
4681 final var biasZ = ba.getElementAtIndex(2);
4682
4683 final var ma = generateMaCommonAxis();
4684 final var sx = ma.getElementAt(0, 0);
4685 final var sy = ma.getElementAt(1, 1);
4686 final var sz = ma.getElementAt(2, 2);
4687 final var mxy = ma.getElementAt(0, 1);
4688 final var mxz = ma.getElementAt(0, 2);
4689 final var myx = ma.getElementAt(1, 0);
4690 final var myz = ma.getElementAt(1, 2);
4691 final var mzx = ma.getElementAt(2, 0);
4692 final var mzy = ma.getElementAt(2, 1);
4693
4694 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4695 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4696 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4697
4698 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz, sx, sy, sz,
4699 mxy, mxz, myx, myz, mzx, mzy);
4700
4701
4702 assertEquals(biasX, calibrator.getBiasX(), 0.0);
4703 assertEquals(biasY, calibrator.getBiasY(), 0.0);
4704 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4705 final var bx1 = calibrator.getBiasXAsAcceleration();
4706 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4707 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4708 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4709 calibrator.getBiasXAsAcceleration(bx2);
4710 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4711 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4712 final var by1 = calibrator.getBiasYAsAcceleration();
4713 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4714 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4715 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4716 calibrator.getBiasYAsAcceleration(by2);
4717 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4718 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4719 final var bz1 = calibrator.getBiasZAsAcceleration();
4720 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4721 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4722 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4723 calibrator.getBiasZAsAcceleration(bz2);
4724 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4725 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4726 final var biasTriad1 = calibrator.getBiasAsTriad();
4727 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4728 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4729 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4730 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4731 final var biasTriad2 = new AccelerationTriad();
4732 calibrator.getBiasAsTriad(biasTriad2);
4733 assertEquals(biasTriad1, biasTriad2);
4734 assertEquals(sx, calibrator.getInitialSx(), 0.0);
4735 assertEquals(sy, calibrator.getInitialSy(), 0.0);
4736 assertEquals(sz, calibrator.getInitialSz(), 0.0);
4737 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4738 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4739 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4740 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4741 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4742 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4743 final var bias1 = calibrator.getBias();
4744 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4745 final var bias2 = new double[3];
4746 calibrator.getBias(bias2);
4747 assertArrayEquals(bias1, bias2, 0.0);
4748 final var b1 = calibrator.getBiasAsMatrix();
4749 assertEquals(b1, ba);
4750 final var b2 = new Matrix(3, 1);
4751 calibrator.getBiasAsMatrix(b2);
4752 assertEquals(b1, b2);
4753 final var ma1 = new Matrix(3, 3);
4754 ma1.setSubmatrix(0, 0, 2, 2,
4755 new double[]{sx, myx, mzx,
4756 mxy, sy, mzy,
4757 mxz, myz, sz});
4758 assertEquals(ma1, calibrator.getInitialMa());
4759 final var ma2 = new Matrix(3, 3);
4760 calibrator.getInitialMa(ma2);
4761 assertEquals(ma1, ma2);
4762 assertNull(calibrator.getMeasurements());
4763 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4764 calibrator.getMeasurementType());
4765 assertFalse(calibrator.isOrderedMeasurementsRequired());
4766 assertFalse(calibrator.isQualityScoresRequired());
4767 assertFalse(calibrator.isCommonAxisUsed());
4768 assertNull(calibrator.getListener());
4769 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
4770 assertFalse(calibrator.isReady());
4771 assertFalse(calibrator.isRunning());
4772 assertNull(calibrator.getEstimatedMa());
4773 assertNull(calibrator.getEstimatedSx());
4774 assertNull(calibrator.getEstimatedSy());
4775 assertNull(calibrator.getEstimatedSz());
4776 assertNull(calibrator.getEstimatedMxy());
4777 assertNull(calibrator.getEstimatedMxz());
4778 assertNull(calibrator.getEstimatedMyx());
4779 assertNull(calibrator.getEstimatedMyz());
4780 assertNull(calibrator.getEstimatedMzx());
4781 assertNull(calibrator.getEstimatedMzy());
4782 assertNull(calibrator.getEstimatedCovariance());
4783 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4784 assertNull(calibrator.getGroundTruthGravityNorm());
4785 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4786 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4787 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4788 }
4789
4790 @Test
4791 void testConstructor48() throws WrongSizeException {
4792 final var ba = generateBa();
4793 final var biasX = ba.getElementAtIndex(0);
4794 final var biasY = ba.getElementAtIndex(1);
4795 final var biasZ = ba.getElementAtIndex(2);
4796
4797 final var ma = generateMaCommonAxis();
4798 final var sx = ma.getElementAt(0, 0);
4799 final var sy = ma.getElementAt(1, 1);
4800 final var sz = ma.getElementAt(2, 2);
4801 final var mxy = ma.getElementAt(0, 1);
4802 final var mxz = ma.getElementAt(0, 2);
4803 final var myx = ma.getElementAt(1, 0);
4804 final var myz = ma.getElementAt(1, 2);
4805 final var mzx = ma.getElementAt(2, 0);
4806 final var mzy = ma.getElementAt(2, 1);
4807
4808 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4809 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4810 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4811
4812 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz, sx, sy, sz,
4813 mxy, mxz, myx, myz, mzx, mzy, this);
4814
4815
4816 assertEquals(biasX, calibrator.getBiasX(), 0.0);
4817 assertEquals(biasY, calibrator.getBiasY(), 0.0);
4818 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4819 final var bx1 = calibrator.getBiasXAsAcceleration();
4820 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4821 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4822 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4823 calibrator.getBiasXAsAcceleration(bx2);
4824 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4825 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4826 final var by1 = calibrator.getBiasYAsAcceleration();
4827 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4828 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4829 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4830 calibrator.getBiasYAsAcceleration(by2);
4831 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4832 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4833 final var bz1 = calibrator.getBiasZAsAcceleration();
4834 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4835 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4836 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4837 calibrator.getBiasZAsAcceleration(bz2);
4838 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4839 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4840 final var biasTriad1 = calibrator.getBiasAsTriad();
4841 assertEquals(biasTriad1.getValueX(), biasX, 0.0);
4842 assertEquals(biasTriad1.getValueY(), biasY, 0.0);
4843 assertEquals(biasTriad1.getValueZ(), biasZ, 0.0);
4844 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4845 final var biasTriad2 = new AccelerationTriad();
4846 calibrator.getBiasAsTriad(biasTriad2);
4847 assertEquals(biasTriad1, biasTriad2);
4848 assertEquals(sx, calibrator.getInitialSx(), 0.0);
4849 assertEquals(sy, calibrator.getInitialSy(), 0.0);
4850 assertEquals(sz, calibrator.getInitialSz(), 0.0);
4851 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4852 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4853 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4854 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4855 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4856 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4857 final var bias1 = calibrator.getBias();
4858 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4859 final var bias2 = new double[3];
4860 calibrator.getBias(bias2);
4861 assertArrayEquals(bias1, bias2, 0.0);
4862 final var b1 = calibrator.getBiasAsMatrix();
4863 assertEquals(b1, ba);
4864 final var b2 = new Matrix(3, 1);
4865 calibrator.getBiasAsMatrix(b2);
4866 assertEquals(b1, b2);
4867 final var ma1 = new Matrix(3, 3);
4868 ma1.setSubmatrix(0, 0, 2, 2,
4869 new double[]{sx, myx, mzx,
4870 mxy, sy, mzy,
4871 mxz, myz, sz});
4872 assertEquals(calibrator.getInitialMa(), ma1);
4873 final var ma2 = new Matrix(3, 3);
4874 calibrator.getInitialMa(ma2);
4875 assertEquals(ma1, ma2);
4876 assertNull(calibrator.getMeasurements());
4877 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4878 calibrator.getMeasurementType());
4879 assertFalse(calibrator.isOrderedMeasurementsRequired());
4880 assertFalse(calibrator.isQualityScoresRequired());
4881 assertFalse(calibrator.isCommonAxisUsed());
4882 assertSame(this, calibrator.getListener());
4883 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
4884 assertFalse(calibrator.isReady());
4885 assertFalse(calibrator.isRunning());
4886 assertNull(calibrator.getEstimatedMa());
4887 assertNull(calibrator.getEstimatedSx());
4888 assertNull(calibrator.getEstimatedSy());
4889 assertNull(calibrator.getEstimatedSz());
4890 assertNull(calibrator.getEstimatedMxy());
4891 assertNull(calibrator.getEstimatedMxz());
4892 assertNull(calibrator.getEstimatedMyx());
4893 assertNull(calibrator.getEstimatedMyz());
4894 assertNull(calibrator.getEstimatedMzx());
4895 assertNull(calibrator.getEstimatedMzy());
4896 assertNull(calibrator.getEstimatedCovariance());
4897 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4898 assertNull(calibrator.getGroundTruthGravityNorm());
4899 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4900 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4901 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4902 }
4903
4904 @Test
4905 void testConstructor49() throws WrongSizeException {
4906
4907 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
4908
4909 final var ba = generateBa();
4910 final var biasX = ba.getElementAtIndex(0);
4911 final var biasY = ba.getElementAtIndex(1);
4912 final var biasZ = ba.getElementAtIndex(2);
4913
4914 final var ma = generateMaCommonAxis();
4915 final var sx = ma.getElementAt(0, 0);
4916 final var sy = ma.getElementAt(1, 1);
4917 final var sz = ma.getElementAt(2, 2);
4918 final var mxy = ma.getElementAt(0, 1);
4919 final var mxz = ma.getElementAt(0, 2);
4920 final var myx = ma.getElementAt(1, 0);
4921 final var myz = ma.getElementAt(1, 2);
4922 final var mzx = ma.getElementAt(2, 0);
4923 final var mzy = ma.getElementAt(2, 1);
4924
4925 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4926 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4927 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4928
4929 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bx, by, bz, sx, sy, sz,
4930 mxy, mxz, myx, myz, mzx, mzy);
4931
4932
4933 assertEquals(biasX, calibrator.getBiasX(), 0.0);
4934 assertEquals(biasY, calibrator.getBiasY(), 0.0);
4935 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4936 final var bx1 = calibrator.getBiasXAsAcceleration();
4937 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4938 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4939 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4940 calibrator.getBiasXAsAcceleration(bx2);
4941 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4942 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4943 final var by1 = calibrator.getBiasYAsAcceleration();
4944 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4945 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4946 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4947 calibrator.getBiasYAsAcceleration(by2);
4948 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4949 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4950 final var bz1 = calibrator.getBiasZAsAcceleration();
4951 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4952 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4953 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4954 calibrator.getBiasZAsAcceleration(bz2);
4955 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4956 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4957 final var biasTriad1 = calibrator.getBiasAsTriad();
4958 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4959 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4960 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4961 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4962 final var biasTriad2 = new AccelerationTriad();
4963 calibrator.getBiasAsTriad(biasTriad2);
4964 assertEquals(biasTriad1, biasTriad2);
4965 assertEquals(sx, calibrator.getInitialSx(), 0.0);
4966 assertEquals(sy, calibrator.getInitialSy(), 0.0);
4967 assertEquals(sz, calibrator.getInitialSz(), 0.0);
4968 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4969 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4970 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4971 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4972 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4973 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4974 final var bias1 = calibrator.getBias();
4975 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4976 final var bias2 = new double[3];
4977 calibrator.getBias(bias2);
4978 assertArrayEquals(bias1, bias2, 0.0);
4979 final var b1 = calibrator.getBiasAsMatrix();
4980 assertEquals(b1, ba);
4981 final var b2 = new Matrix(3, 1);
4982 calibrator.getBiasAsMatrix(b2);
4983 assertEquals(b1, b2);
4984 final var ma1 = new Matrix(3, 3);
4985 ma1.setSubmatrix(0, 0, 2, 2,
4986 new double[]{sx, myx, mzx,
4987 mxy, sy, mzy,
4988 mxz, myz, sz});
4989 assertEquals(ma1, calibrator.getInitialMa());
4990 final var ma2 = new Matrix(3, 3);
4991 calibrator.getInitialMa(ma2);
4992 assertEquals(ma1, ma2);
4993 assertSame(measurements, calibrator.getMeasurements());
4994 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4995 calibrator.getMeasurementType());
4996 assertFalse(calibrator.isOrderedMeasurementsRequired());
4997 assertFalse(calibrator.isQualityScoresRequired());
4998 assertFalse(calibrator.isCommonAxisUsed());
4999 assertNull(calibrator.getListener());
5000 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
5001 assertFalse(calibrator.isReady());
5002 assertFalse(calibrator.isRunning());
5003 assertNull(calibrator.getEstimatedMa());
5004 assertNull(calibrator.getEstimatedSx());
5005 assertNull(calibrator.getEstimatedSy());
5006 assertNull(calibrator.getEstimatedSz());
5007 assertNull(calibrator.getEstimatedMxy());
5008 assertNull(calibrator.getEstimatedMxz());
5009 assertNull(calibrator.getEstimatedMyx());
5010 assertNull(calibrator.getEstimatedMyz());
5011 assertNull(calibrator.getEstimatedMzx());
5012 assertNull(calibrator.getEstimatedMzy());
5013 assertNull(calibrator.getEstimatedCovariance());
5014 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5015 assertNull(calibrator.getGroundTruthGravityNorm());
5016 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5017 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5018 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5019 }
5020
5021 @Test
5022 void testConstructor50() throws WrongSizeException {
5023
5024 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
5025
5026 final var ba = generateBa();
5027 final var biasX = ba.getElementAtIndex(0);
5028 final var biasY = ba.getElementAtIndex(1);
5029 final var biasZ = ba.getElementAtIndex(2);
5030
5031 final var ma = generateMaCommonAxis();
5032 final var sx = ma.getElementAt(0, 0);
5033 final var sy = ma.getElementAt(1, 1);
5034 final var sz = ma.getElementAt(2, 2);
5035 final var mxy = ma.getElementAt(0, 1);
5036 final var mxz = ma.getElementAt(0, 2);
5037 final var myx = ma.getElementAt(1, 0);
5038 final var myz = ma.getElementAt(1, 2);
5039 final var mzx = ma.getElementAt(2, 0);
5040 final var mzy = ma.getElementAt(2, 1);
5041
5042 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5043 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5044 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5045
5046 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bx, by, bz, sx, sy, sz,
5047 mxy, mxz, myx, myz, mzx, mzy, this);
5048
5049
5050 assertEquals(biasX, calibrator.getBiasX(), 0.0);
5051 assertEquals(biasY, calibrator.getBiasY(), 0.0);
5052 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5053 final var bx1 = calibrator.getBiasXAsAcceleration();
5054 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5055 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5056 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5057 calibrator.getBiasXAsAcceleration(bx2);
5058 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5059 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5060 final var by1 = calibrator.getBiasYAsAcceleration();
5061 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5062 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5063 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5064 calibrator.getBiasYAsAcceleration(by2);
5065 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5066 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5067 final var bz1 = calibrator.getBiasZAsAcceleration();
5068 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5069 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5070 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5071 calibrator.getBiasZAsAcceleration(bz2);
5072 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5073 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5074 final var biasTriad1 = calibrator.getBiasAsTriad();
5075 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5076 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5077 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5078 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5079 final var biasTriad2 = new AccelerationTriad();
5080 calibrator.getBiasAsTriad(biasTriad2);
5081 assertEquals(biasTriad1, biasTriad2);
5082 assertEquals(sx, calibrator.getInitialSx(), 0.0);
5083 assertEquals(sy, calibrator.getInitialSy(), 0.0);
5084 assertEquals(sz, calibrator.getInitialSz(), 0.0);
5085 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
5086 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
5087 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
5088 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
5089 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
5090 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
5091 final var bias1 = calibrator.getBias();
5092 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5093 final var bias2 = new double[3];
5094 calibrator.getBias(bias2);
5095 assertArrayEquals(bias1, bias2, 0.0);
5096 final var b1 = calibrator.getBiasAsMatrix();
5097 assertEquals(b1, ba);
5098 final var b2 = new Matrix(3, 1);
5099 calibrator.getBiasAsMatrix(b2);
5100 assertEquals(b1, b2);
5101 final var ma1 = new Matrix(3, 3);
5102 ma1.setSubmatrix(0, 0, 2, 2,
5103 new double[]{sx, myx, mzx,
5104 mxy, sy, mzy,
5105 mxz, myz, sz});
5106 assertEquals(ma1, calibrator.getInitialMa());
5107 final var ma2 = new Matrix(3, 3);
5108 calibrator.getInitialMa(ma2);
5109 assertEquals(ma1, ma2);
5110 assertSame(measurements, calibrator.getMeasurements());
5111 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5112 calibrator.getMeasurementType());
5113 assertFalse(calibrator.isOrderedMeasurementsRequired());
5114 assertFalse(calibrator.isQualityScoresRequired());
5115 assertFalse(calibrator.isCommonAxisUsed());
5116 assertSame(this, calibrator.getListener());
5117 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
5118 assertFalse(calibrator.isReady());
5119 assertFalse(calibrator.isRunning());
5120 assertNull(calibrator.getEstimatedMa());
5121 assertNull(calibrator.getEstimatedSx());
5122 assertNull(calibrator.getEstimatedSy());
5123 assertNull(calibrator.getEstimatedSz());
5124 assertNull(calibrator.getEstimatedMxy());
5125 assertNull(calibrator.getEstimatedMxz());
5126 assertNull(calibrator.getEstimatedMyx());
5127 assertNull(calibrator.getEstimatedMyz());
5128 assertNull(calibrator.getEstimatedMzx());
5129 assertNull(calibrator.getEstimatedMzy());
5130 assertNull(calibrator.getEstimatedCovariance());
5131 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5132 assertNull(calibrator.getGroundTruthGravityNorm());
5133 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5134 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5135 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5136 }
5137
5138 @Test
5139 void testConstructor51() throws WrongSizeException {
5140 final var ba = generateBa();
5141 final var biasX = ba.getElementAtIndex(0);
5142 final var biasY = ba.getElementAtIndex(1);
5143 final var biasZ = ba.getElementAtIndex(2);
5144
5145 final var ma = generateMaCommonAxis();
5146 final var sx = ma.getElementAt(0, 0);
5147 final var sy = ma.getElementAt(1, 1);
5148 final var sz = ma.getElementAt(2, 2);
5149 final var mxy = ma.getElementAt(0, 1);
5150 final var mxz = ma.getElementAt(0, 2);
5151 final var myx = ma.getElementAt(1, 0);
5152 final var myz = ma.getElementAt(1, 2);
5153 final var mzx = ma.getElementAt(2, 0);
5154 final var mzy = ma.getElementAt(2, 1);
5155
5156 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5157 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5158 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5159
5160 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bx, by, bz,
5161 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5162
5163
5164 assertEquals(biasX, calibrator.getBiasX(), 0.0);
5165 assertEquals(biasY, calibrator.getBiasY(), 0.0);
5166 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5167 final var bx1 = calibrator.getBiasXAsAcceleration();
5168 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5169 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5170 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5171 calibrator.getBiasXAsAcceleration(bx2);
5172 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5173 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5174 final var by1 = calibrator.getBiasYAsAcceleration();
5175 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5176 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5177 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5178 calibrator.getBiasYAsAcceleration(by2);
5179 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5180 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5181 final var bz1 = calibrator.getBiasZAsAcceleration();
5182 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5183 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5184 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5185 calibrator.getBiasZAsAcceleration(bz2);
5186 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5187 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5188 final var biasTriad1 = calibrator.getBiasAsTriad();
5189 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5190 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5191 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5192 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5193 final var biasTriad2 = new AccelerationTriad();
5194 calibrator.getBiasAsTriad(biasTriad2);
5195 assertEquals(biasTriad1, biasTriad2);
5196 assertEquals(sx, calibrator.getInitialSx(), 0.0);
5197 assertEquals(sy, calibrator.getInitialSy(), 0.0);
5198 assertEquals(sz, calibrator.getInitialSz(), 0.0);
5199 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
5200 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
5201 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
5202 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
5203 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
5204 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
5205 final var bias1 = calibrator.getBias();
5206 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5207 final var bias2 = new double[3];
5208 calibrator.getBias(bias2);
5209 assertArrayEquals(bias1, bias2, 0.0);
5210 final var b1 = calibrator.getBiasAsMatrix();
5211 assertEquals(b1, ba);
5212 final var b2 = new Matrix(3, 1);
5213 calibrator.getBiasAsMatrix(b2);
5214 assertEquals(b1, b2);
5215 final var ma1 = new Matrix(3, 3);
5216 ma1.setSubmatrix(0, 0, 2, 2,
5217 new double[]{sx, myx, mzx,
5218 mxy, sy, mzy,
5219 mxz, myz, sz});
5220 assertEquals(ma1, calibrator.getInitialMa());
5221 final var ma2 = new Matrix(3, 3);
5222 calibrator.getInitialMa(ma2);
5223 assertEquals(ma1, ma2);
5224 assertNull(calibrator.getMeasurements());
5225 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5226 calibrator.getMeasurementType());
5227 assertFalse(calibrator.isOrderedMeasurementsRequired());
5228 assertFalse(calibrator.isQualityScoresRequired());
5229 assertTrue(calibrator.isCommonAxisUsed());
5230 assertNull(calibrator.getListener());
5231 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
5232 assertFalse(calibrator.isReady());
5233 assertFalse(calibrator.isRunning());
5234 assertNull(calibrator.getEstimatedMa());
5235 assertNull(calibrator.getEstimatedSx());
5236 assertNull(calibrator.getEstimatedSy());
5237 assertNull(calibrator.getEstimatedSz());
5238 assertNull(calibrator.getEstimatedMxy());
5239 assertNull(calibrator.getEstimatedMxz());
5240 assertNull(calibrator.getEstimatedMyx());
5241 assertNull(calibrator.getEstimatedMyz());
5242 assertNull(calibrator.getEstimatedMzx());
5243 assertNull(calibrator.getEstimatedMzy());
5244 assertNull(calibrator.getEstimatedCovariance());
5245 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5246 assertNull(calibrator.getGroundTruthGravityNorm());
5247 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5248 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5249 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5250 }
5251
5252 @Test
5253 void testConstructor52() throws WrongSizeException {
5254 final var ba = generateBa();
5255 final var biasX = ba.getElementAtIndex(0);
5256 final var biasY = ba.getElementAtIndex(1);
5257 final var biasZ = ba.getElementAtIndex(2);
5258
5259 final var ma = generateMaCommonAxis();
5260 final var sx = ma.getElementAt(0, 0);
5261 final var sy = ma.getElementAt(1, 1);
5262 final var sz = ma.getElementAt(2, 2);
5263 final var mxy = ma.getElementAt(0, 1);
5264 final var mxz = ma.getElementAt(0, 2);
5265 final var myx = ma.getElementAt(1, 0);
5266 final var myz = ma.getElementAt(1, 2);
5267 final var mzx = ma.getElementAt(2, 0);
5268 final var mzy = ma.getElementAt(2, 1);
5269
5270 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5271 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5272 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5273
5274 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bx, by, bz,
5275 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5276
5277
5278 assertEquals(biasX, calibrator.getBiasX(), 0.0);
5279 assertEquals(biasY, calibrator.getBiasY(), 0.0);
5280 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5281 final var bx1 = calibrator.getBiasXAsAcceleration();
5282 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5283 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5284 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5285 calibrator.getBiasXAsAcceleration(bx2);
5286 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5287 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5288 final var by1 = calibrator.getBiasYAsAcceleration();
5289 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5290 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5291 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5292 calibrator.getBiasYAsAcceleration(by2);
5293 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5294 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5295 final var bz1 = calibrator.getBiasZAsAcceleration();
5296 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5297 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5298 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5299 calibrator.getBiasZAsAcceleration(bz2);
5300 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5301 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5302 final var biasTriad1 = calibrator.getBiasAsTriad();
5303 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5304 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5305 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5306 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5307 final var biasTriad2 = new AccelerationTriad();
5308 calibrator.getBiasAsTriad(biasTriad2);
5309 assertEquals(biasTriad1, biasTriad2);
5310 assertEquals(sx, calibrator.getInitialSx(), 0.0);
5311 assertEquals(sy, calibrator.getInitialSy(), 0.0);
5312 assertEquals(sz, calibrator.getInitialSz(), 0.0);
5313 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
5314 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
5315 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
5316 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
5317 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
5318 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
5319 final var bias1 = calibrator.getBias();
5320 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5321 final var bias2 = new double[3];
5322 calibrator.getBias(bias2);
5323 assertArrayEquals(bias1, bias2, 0.0);
5324 final var b1 = calibrator.getBiasAsMatrix();
5325 assertEquals(b1, ba);
5326 final var b2 = new Matrix(3, 1);
5327 calibrator.getBiasAsMatrix(b2);
5328 assertEquals(b1, b2);
5329 final var ma1 = new Matrix(3, 3);
5330 ma1.setSubmatrix(0, 0, 2, 2,
5331 new double[]{sx, myx, mzx,
5332 mxy, sy, mzy,
5333 mxz, myz, sz});
5334 assertEquals(ma1, calibrator.getInitialMa());
5335 final var ma2 = new Matrix(3, 3);
5336 calibrator.getInitialMa(ma2);
5337 assertEquals(ma1, ma2);
5338 assertNull(calibrator.getMeasurements());
5339 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5340 calibrator.getMeasurementType());
5341 assertFalse(calibrator.isOrderedMeasurementsRequired());
5342 assertFalse(calibrator.isQualityScoresRequired());
5343 assertTrue(calibrator.isCommonAxisUsed());
5344 assertSame(this, calibrator.getListener());
5345 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
5346 assertFalse(calibrator.isReady());
5347 assertFalse(calibrator.isRunning());
5348 assertNull(calibrator.getEstimatedMa());
5349 assertNull(calibrator.getEstimatedSx());
5350 assertNull(calibrator.getEstimatedSy());
5351 assertNull(calibrator.getEstimatedSz());
5352 assertNull(calibrator.getEstimatedMxy());
5353 assertNull(calibrator.getEstimatedMxz());
5354 assertNull(calibrator.getEstimatedMyx());
5355 assertNull(calibrator.getEstimatedMyz());
5356 assertNull(calibrator.getEstimatedMzx());
5357 assertNull(calibrator.getEstimatedMzy());
5358 assertNull(calibrator.getEstimatedCovariance());
5359 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5360 assertNull(calibrator.getGroundTruthGravityNorm());
5361 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5362 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5363 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5364 }
5365
5366 @Test
5367 void testConstructor53() throws WrongSizeException {
5368 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
5369
5370 final var ba = generateBa();
5371 final var biasX = ba.getElementAtIndex(0);
5372 final var biasY = ba.getElementAtIndex(1);
5373 final var biasZ = ba.getElementAtIndex(2);
5374
5375 final var ma = generateMaCommonAxis();
5376 final var sx = ma.getElementAt(0, 0);
5377 final var sy = ma.getElementAt(1, 1);
5378 final var sz = ma.getElementAt(2, 2);
5379 final var mxy = ma.getElementAt(0, 1);
5380 final var mxz = ma.getElementAt(0, 2);
5381 final var myx = ma.getElementAt(1, 0);
5382 final var myz = ma.getElementAt(1, 2);
5383 final var mzx = ma.getElementAt(2, 0);
5384 final var mzy = ma.getElementAt(2, 1);
5385
5386 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5387 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5388 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5389
5390 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
5391 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5392
5393
5394 assertEquals(biasX, calibrator.getBiasX(), 0.0);
5395 assertEquals(biasY, calibrator.getBiasY(), 0.0);
5396 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5397 final var bx1 = calibrator.getBiasXAsAcceleration();
5398 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5399 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5400 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5401 calibrator.getBiasXAsAcceleration(bx2);
5402 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5403 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5404 final var by1 = calibrator.getBiasYAsAcceleration();
5405 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5406 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5407 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5408 calibrator.getBiasYAsAcceleration(by2);
5409 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5410 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5411 final var bz1 = calibrator.getBiasZAsAcceleration();
5412 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5413 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5414 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5415 calibrator.getBiasZAsAcceleration(bz2);
5416 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5417 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5418 final var biasTriad1 = calibrator.getBiasAsTriad();
5419 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5420 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5421 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5422 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5423 final var biasTriad2 = new AccelerationTriad();
5424 calibrator.getBiasAsTriad(biasTriad2);
5425 assertEquals(biasTriad1, biasTriad2);
5426 assertEquals(sx, calibrator.getInitialSx(), 0.0);
5427 assertEquals(sy, calibrator.getInitialSy(), 0.0);
5428 assertEquals(sz, calibrator.getInitialSz(), 0.0);
5429 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
5430 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
5431 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
5432 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
5433 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
5434 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
5435 final var bias1 = calibrator.getBias();
5436 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5437 final var bias2 = new double[3];
5438 calibrator.getBias(bias2);
5439 assertArrayEquals(bias1, bias2, 0.0);
5440 final var b1 = calibrator.getBiasAsMatrix();
5441 assertEquals(b1, ba);
5442 final var b2 = new Matrix(3, 1);
5443 calibrator.getBiasAsMatrix(b2);
5444 assertEquals(b1, b2);
5445 final var ma1 = new Matrix(3, 3);
5446 ma1.setSubmatrix(0, 0, 2, 2,
5447 new double[]{sx, myx, mzx,
5448 mxy, sy, mzy,
5449 mxz, myz, sz});
5450 assertEquals(ma1, calibrator.getInitialMa());
5451 final var ma2 = new Matrix(3, 3);
5452 calibrator.getInitialMa(ma2);
5453 assertEquals(ma1, ma2);
5454 assertSame(measurements, calibrator.getMeasurements());
5455 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5456 calibrator.getMeasurementType());
5457 assertFalse(calibrator.isOrderedMeasurementsRequired());
5458 assertFalse(calibrator.isQualityScoresRequired());
5459 assertTrue(calibrator.isCommonAxisUsed());
5460 assertNull(calibrator.getListener());
5461 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
5462 assertFalse(calibrator.isReady());
5463 assertFalse(calibrator.isRunning());
5464 assertNull(calibrator.getEstimatedMa());
5465 assertNull(calibrator.getEstimatedSx());
5466 assertNull(calibrator.getEstimatedSy());
5467 assertNull(calibrator.getEstimatedSz());
5468 assertNull(calibrator.getEstimatedMxy());
5469 assertNull(calibrator.getEstimatedMxz());
5470 assertNull(calibrator.getEstimatedMyx());
5471 assertNull(calibrator.getEstimatedMyz());
5472 assertNull(calibrator.getEstimatedMzx());
5473 assertNull(calibrator.getEstimatedMzy());
5474 assertNull(calibrator.getEstimatedCovariance());
5475 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5476 assertNull(calibrator.getGroundTruthGravityNorm());
5477 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5478 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5479 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5480 }
5481
5482 @Test
5483 void testConstructor54() throws WrongSizeException {
5484 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
5485
5486 final var ba = generateBa();
5487 final var biasX = ba.getElementAtIndex(0);
5488 final var biasY = ba.getElementAtIndex(1);
5489 final var biasZ = ba.getElementAtIndex(2);
5490
5491 final var ma = generateMaCommonAxis();
5492 final var sx = ma.getElementAt(0, 0);
5493 final var sy = ma.getElementAt(1, 1);
5494 final var sz = ma.getElementAt(2, 2);
5495 final var mxy = ma.getElementAt(0, 1);
5496 final var mxz = ma.getElementAt(0, 2);
5497 final var myx = ma.getElementAt(1, 0);
5498 final var myz = ma.getElementAt(1, 2);
5499 final var mzx = ma.getElementAt(2, 0);
5500 final var mzy = ma.getElementAt(2, 1);
5501
5502 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5503 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5504 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5505
5506 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
5507 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5508
5509
5510 assertEquals(biasX, calibrator.getBiasX(), 0.0);
5511 assertEquals(biasY, calibrator.getBiasY(), 0.0);
5512 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5513 final var bx1 = calibrator.getBiasXAsAcceleration();
5514 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5515 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5516 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5517 calibrator.getBiasXAsAcceleration(bx2);
5518 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5519 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5520 final var by1 = calibrator.getBiasYAsAcceleration();
5521 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5522 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5523 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5524 calibrator.getBiasYAsAcceleration(by2);
5525 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5526 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5527 final var bz1 = calibrator.getBiasZAsAcceleration();
5528 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5529 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5530 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5531 calibrator.getBiasZAsAcceleration(bz2);
5532 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5533 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5534 final var biasTriad1 = calibrator.getBiasAsTriad();
5535 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5536 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5537 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5538 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5539 final var biasTriad2 = new AccelerationTriad();
5540 calibrator.getBiasAsTriad(biasTriad2);
5541 assertEquals(biasTriad1, biasTriad2);
5542 assertEquals(sx, calibrator.getInitialSx(), 0.0);
5543 assertEquals(sy, calibrator.getInitialSy(), 0.0);
5544 assertEquals(sz, calibrator.getInitialSz(), 0.0);
5545 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
5546 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
5547 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
5548 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
5549 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
5550 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
5551 final var bias1 = calibrator.getBias();
5552 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5553 final var bias2 = new double[3];
5554 calibrator.getBias(bias2);
5555 assertArrayEquals(bias1, bias2, 0.0);
5556 final var b1 = calibrator.getBiasAsMatrix();
5557 assertEquals(b1, ba);
5558 final var b2 = new Matrix(3, 1);
5559 calibrator.getBiasAsMatrix(b2);
5560 assertEquals(b1, b2);
5561 final var ma1 = new Matrix(3, 3);
5562 ma1.setSubmatrix(0, 0, 2, 2,
5563 new double[]{sx, myx, mzx,
5564 mxy, sy, mzy,
5565 mxz, myz, sz});
5566 assertEquals(ma1, calibrator.getInitialMa());
5567 final var ma2 = new Matrix(3, 3);
5568 calibrator.getInitialMa(ma2);
5569 assertEquals(ma1, ma2);
5570 assertSame(measurements, calibrator.getMeasurements());
5571 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5572 calibrator.getMeasurementType());
5573 assertFalse(calibrator.isOrderedMeasurementsRequired());
5574 assertFalse(calibrator.isQualityScoresRequired());
5575 assertTrue(calibrator.isCommonAxisUsed());
5576 assertSame(this, calibrator.getListener());
5577 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
5578 assertFalse(calibrator.isReady());
5579 assertFalse(calibrator.isRunning());
5580 assertNull(calibrator.getEstimatedMa());
5581 assertNull(calibrator.getEstimatedSx());
5582 assertNull(calibrator.getEstimatedSy());
5583 assertNull(calibrator.getEstimatedSz());
5584 assertNull(calibrator.getEstimatedMxy());
5585 assertNull(calibrator.getEstimatedMxz());
5586 assertNull(calibrator.getEstimatedMyx());
5587 assertNull(calibrator.getEstimatedMyz());
5588 assertNull(calibrator.getEstimatedMzx());
5589 assertNull(calibrator.getEstimatedMzy());
5590 assertNull(calibrator.getEstimatedCovariance());
5591 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5592 assertNull(calibrator.getGroundTruthGravityNorm());
5593 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5594 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5595 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5596 }
5597
5598 @Test
5599 void testConstructor55() throws WrongSizeException {
5600 final var ba = generateBa();
5601 final var bias = ba.getBuffer();
5602 final var biasX = ba.getElementAtIndex(0);
5603 final var biasY = ba.getElementAtIndex(1);
5604 final var biasZ = ba.getElementAtIndex(2);
5605
5606 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bias);
5607
5608
5609 assertEquals(biasX, calibrator.getBiasX(), 0.0);
5610 assertEquals(biasY, calibrator.getBiasY(), 0.0);
5611 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5612 final var bx1 = calibrator.getBiasXAsAcceleration();
5613 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5614 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5615 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5616 calibrator.getBiasXAsAcceleration(bx2);
5617 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5618 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5619 final var by1 = calibrator.getBiasYAsAcceleration();
5620 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5621 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5622 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5623 calibrator.getBiasYAsAcceleration(by2);
5624 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5625 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5626 final var bz1 = calibrator.getBiasZAsAcceleration();
5627 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5628 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5629 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5630 calibrator.getBiasZAsAcceleration(bz2);
5631 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5632 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5633 final var biasTriad1 = calibrator.getBiasAsTriad();
5634 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5635 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5636 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5637 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5638 final var biasTriad2 = new AccelerationTriad();
5639 calibrator.getBiasAsTriad(biasTriad2);
5640 assertEquals(biasTriad1, biasTriad2);
5641 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
5642 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
5643 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
5644 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
5645 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
5646 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
5647 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
5648 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
5649 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
5650 final var bias1 = calibrator.getBias();
5651 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5652 final var bias2 = new double[3];
5653 calibrator.getBias(bias2);
5654 assertArrayEquals(bias1, bias2, 0.0);
5655 final var b1 = calibrator.getBiasAsMatrix();
5656 assertEquals(b1, ba);
5657 final var b2 = new Matrix(3, 1);
5658 calibrator.getBiasAsMatrix(b2);
5659 assertEquals(b1, b2);
5660 final var ma1 = calibrator.getInitialMa();
5661 assertEquals(new Matrix(3, 3), ma1);
5662 final var ma2 = new Matrix(3, 3);
5663 calibrator.getInitialMa(ma2);
5664 assertEquals(ma1, ma2);
5665 assertNull(calibrator.getMeasurements());
5666 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5667 calibrator.getMeasurementType());
5668 assertFalse(calibrator.isOrderedMeasurementsRequired());
5669 assertFalse(calibrator.isQualityScoresRequired());
5670 assertFalse(calibrator.isCommonAxisUsed());
5671 assertNull(calibrator.getListener());
5672 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
5673 assertFalse(calibrator.isReady());
5674 assertFalse(calibrator.isRunning());
5675 assertNull(calibrator.getEstimatedMa());
5676 assertNull(calibrator.getEstimatedSx());
5677 assertNull(calibrator.getEstimatedSy());
5678 assertNull(calibrator.getEstimatedSz());
5679 assertNull(calibrator.getEstimatedMxy());
5680 assertNull(calibrator.getEstimatedMxz());
5681 assertNull(calibrator.getEstimatedMyx());
5682 assertNull(calibrator.getEstimatedMyz());
5683 assertNull(calibrator.getEstimatedMzx());
5684 assertNull(calibrator.getEstimatedMzy());
5685 assertNull(calibrator.getEstimatedCovariance());
5686 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5687 assertNull(calibrator.getGroundTruthGravityNorm());
5688 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5689 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5690 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5691
5692
5693 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
5694 new double[1]));
5695 }
5696
5697 @Test
5698 void testConstructor56() throws WrongSizeException {
5699 final var ba = generateBa();
5700 final var bias = ba.getBuffer();
5701 final var biasX = ba.getElementAtIndex(0);
5702 final var biasY = ba.getElementAtIndex(1);
5703 final var biasZ = ba.getElementAtIndex(2);
5704
5705 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bias, this);
5706
5707
5708 assertEquals(biasX, calibrator.getBiasX(), 0.0);
5709 assertEquals(biasY, calibrator.getBiasY(), 0.0);
5710 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5711 final var bx1 = calibrator.getBiasXAsAcceleration();
5712 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5713 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5714 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5715 calibrator.getBiasXAsAcceleration(bx2);
5716 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5717 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5718 final var by1 = calibrator.getBiasYAsAcceleration();
5719 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5720 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5721 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5722 calibrator.getBiasYAsAcceleration(by2);
5723 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5724 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5725 final var bz1 = calibrator.getBiasZAsAcceleration();
5726 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5727 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5728 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5729 calibrator.getBiasZAsAcceleration(bz2);
5730 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5731 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5732 final var biasTriad1 = calibrator.getBiasAsTriad();
5733 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5734 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5735 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5736 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5737 final var biasTriad2 = new AccelerationTriad();
5738 calibrator.getBiasAsTriad(biasTriad2);
5739 assertEquals(biasTriad1, biasTriad2);
5740 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
5741 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
5742 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
5743 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
5744 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
5745 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
5746 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
5747 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
5748 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
5749 final var bias1 = calibrator.getBias();
5750 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5751 final var bias2 = new double[3];
5752 calibrator.getBias(bias2);
5753 assertArrayEquals(bias1, bias2, 0.0);
5754 final var b1 = calibrator.getBiasAsMatrix();
5755 assertEquals(b1, ba);
5756 final var b2 = new Matrix(3, 1);
5757 calibrator.getBiasAsMatrix(b2);
5758 assertEquals(b1, b2);
5759 final var ma1 = calibrator.getInitialMa();
5760 assertEquals(new Matrix(3, 3), ma1);
5761 final var ma2 = new Matrix(3, 3);
5762 calibrator.getInitialMa(ma2);
5763 assertEquals(ma1, ma2);
5764 assertNull(calibrator.getMeasurements());
5765 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5766 calibrator.getMeasurementType());
5767 assertFalse(calibrator.isOrderedMeasurementsRequired());
5768 assertFalse(calibrator.isQualityScoresRequired());
5769 assertFalse(calibrator.isCommonAxisUsed());
5770 assertSame(this, calibrator.getListener());
5771 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
5772 assertFalse(calibrator.isReady());
5773 assertFalse(calibrator.isRunning());
5774 assertNull(calibrator.getEstimatedMa());
5775 assertNull(calibrator.getEstimatedSx());
5776 assertNull(calibrator.getEstimatedSy());
5777 assertNull(calibrator.getEstimatedSz());
5778 assertNull(calibrator.getEstimatedMxy());
5779 assertNull(calibrator.getEstimatedMxz());
5780 assertNull(calibrator.getEstimatedMyx());
5781 assertNull(calibrator.getEstimatedMyz());
5782 assertNull(calibrator.getEstimatedMzx());
5783 assertNull(calibrator.getEstimatedMzy());
5784 assertNull(calibrator.getEstimatedCovariance());
5785 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5786 assertNull(calibrator.getGroundTruthGravityNorm());
5787 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5788 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5789 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5790
5791
5792 assertThrows(IllegalArgumentException.class,
5793 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(new double[1], this));
5794 }
5795
5796 @Test
5797 void testConstructor57() throws WrongSizeException {
5798 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
5799
5800 final var ba = generateBa();
5801 final var bias = ba.getBuffer();
5802 final var biasX = ba.getElementAtIndex(0);
5803 final var biasY = ba.getElementAtIndex(1);
5804 final var biasZ = ba.getElementAtIndex(2);
5805
5806 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bias);
5807
5808
5809 assertEquals(biasX, calibrator.getBiasX(), 0.0);
5810 assertEquals(biasY, calibrator.getBiasY(), 0.0);
5811 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5812 final var bx1 = calibrator.getBiasXAsAcceleration();
5813 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5814 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5815 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5816 calibrator.getBiasXAsAcceleration(bx2);
5817 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5818 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5819 final var by1 = calibrator.getBiasYAsAcceleration();
5820 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5821 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5822 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5823 calibrator.getBiasYAsAcceleration(by2);
5824 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5825 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5826 final var bz1 = calibrator.getBiasZAsAcceleration();
5827 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5828 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5829 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5830 calibrator.getBiasZAsAcceleration(bz2);
5831 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5832 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5833 final var biasTriad1 = calibrator.getBiasAsTriad();
5834 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5835 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5836 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5837 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5838 final var biasTriad2 = new AccelerationTriad();
5839 calibrator.getBiasAsTriad(biasTriad2);
5840 assertEquals(biasTriad1, biasTriad2);
5841 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
5842 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
5843 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
5844 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
5845 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
5846 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
5847 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
5848 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
5849 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
5850 final var bias1 = calibrator.getBias();
5851 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5852 final var bias2 = new double[3];
5853 calibrator.getBias(bias2);
5854 assertArrayEquals(bias1, bias2, 0.0);
5855 final var b1 = calibrator.getBiasAsMatrix();
5856 assertEquals(b1, ba);
5857 final var b2 = new Matrix(3, 1);
5858 calibrator.getBiasAsMatrix(b2);
5859 assertEquals(b1, b2);
5860 final var ma1 = calibrator.getInitialMa();
5861 assertEquals(new Matrix(3, 3), ma1);
5862 final var ma2 = new Matrix(3, 3);
5863 calibrator.getInitialMa(ma2);
5864 assertEquals(ma1, ma2);
5865 assertSame(measurements, calibrator.getMeasurements());
5866 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5867 calibrator.getMeasurementType());
5868 assertFalse(calibrator.isOrderedMeasurementsRequired());
5869 assertFalse(calibrator.isQualityScoresRequired());
5870 assertFalse(calibrator.isCommonAxisUsed());
5871 assertNull(calibrator.getListener());
5872 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
5873 assertFalse(calibrator.isReady());
5874 assertFalse(calibrator.isRunning());
5875 assertNull(calibrator.getEstimatedMa());
5876 assertNull(calibrator.getEstimatedSx());
5877 assertNull(calibrator.getEstimatedSy());
5878 assertNull(calibrator.getEstimatedSz());
5879 assertNull(calibrator.getEstimatedMxy());
5880 assertNull(calibrator.getEstimatedMxz());
5881 assertNull(calibrator.getEstimatedMyx());
5882 assertNull(calibrator.getEstimatedMyz());
5883 assertNull(calibrator.getEstimatedMzx());
5884 assertNull(calibrator.getEstimatedMzy());
5885 assertNull(calibrator.getEstimatedCovariance());
5886 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5887 assertNull(calibrator.getGroundTruthGravityNorm());
5888 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5889 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5890 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5891
5892
5893 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
5894 measurements, new double[1]));
5895 }
5896
5897 @Test
5898 void testConstructor58() throws WrongSizeException {
5899 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
5900
5901 final var ba = generateBa();
5902 final var bias = ba.getBuffer();
5903 final var biasX = ba.getElementAtIndex(0);
5904 final var biasY = ba.getElementAtIndex(1);
5905 final var biasZ = ba.getElementAtIndex(2);
5906
5907 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bias, this);
5908
5909
5910 assertEquals(biasX, calibrator.getBiasX(), 0.0);
5911 assertEquals(biasY, calibrator.getBiasY(), 0.0);
5912 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5913 final var bx1 = calibrator.getBiasXAsAcceleration();
5914 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5915 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5916 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5917 calibrator.getBiasXAsAcceleration(bx2);
5918 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5919 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5920 final var by1 = calibrator.getBiasYAsAcceleration();
5921 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5922 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5923 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5924 calibrator.getBiasYAsAcceleration(by2);
5925 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5926 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5927 final var bz1 = calibrator.getBiasZAsAcceleration();
5928 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5929 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5930 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5931 calibrator.getBiasZAsAcceleration(bz2);
5932 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5933 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5934 final var biasTriad1 = calibrator.getBiasAsTriad();
5935 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5936 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5937 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5938 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5939 final var biasTriad2 = new AccelerationTriad();
5940 calibrator.getBiasAsTriad(biasTriad2);
5941 assertEquals(biasTriad1, biasTriad2);
5942 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
5943 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
5944 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
5945 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
5946 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
5947 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
5948 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
5949 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
5950 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
5951 final var bias1 = calibrator.getBias();
5952 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5953 final var bias2 = new double[3];
5954 calibrator.getBias(bias2);
5955 assertArrayEquals(bias1, bias2, 0.0);
5956 final var b1 = calibrator.getBiasAsMatrix();
5957 assertEquals(b1, ba);
5958 final var b2 = new Matrix(3, 1);
5959 calibrator.getBiasAsMatrix(b2);
5960 assertEquals(b1, b2);
5961 final var ma1 = calibrator.getInitialMa();
5962 assertEquals(new Matrix(3, 3), ma1);
5963 final var ma2 = new Matrix(3, 3);
5964 calibrator.getInitialMa(ma2);
5965 assertEquals(ma1, ma2);
5966 assertSame(measurements, calibrator.getMeasurements());
5967 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5968 calibrator.getMeasurementType());
5969 assertFalse(calibrator.isOrderedMeasurementsRequired());
5970 assertFalse(calibrator.isQualityScoresRequired());
5971 assertFalse(calibrator.isCommonAxisUsed());
5972 assertSame(this, calibrator.getListener());
5973 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
5974 assertFalse(calibrator.isReady());
5975 assertFalse(calibrator.isRunning());
5976 assertNull(calibrator.getEstimatedMa());
5977 assertNull(calibrator.getEstimatedSx());
5978 assertNull(calibrator.getEstimatedSy());
5979 assertNull(calibrator.getEstimatedSz());
5980 assertNull(calibrator.getEstimatedMxy());
5981 assertNull(calibrator.getEstimatedMxz());
5982 assertNull(calibrator.getEstimatedMyx());
5983 assertNull(calibrator.getEstimatedMyz());
5984 assertNull(calibrator.getEstimatedMzx());
5985 assertNull(calibrator.getEstimatedMzy());
5986 assertNull(calibrator.getEstimatedCovariance());
5987 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5988 assertNull(calibrator.getGroundTruthGravityNorm());
5989 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5990 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5991 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5992
5993
5994 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
5995 measurements, new double[1], this));
5996 }
5997
5998 @Test
5999 void testConstructor59() throws WrongSizeException {
6000 final var ba = generateBa();
6001 final var bias = ba.getBuffer();
6002 final var biasX = ba.getElementAtIndex(0);
6003 final var biasY = ba.getElementAtIndex(1);
6004 final var biasZ = ba.getElementAtIndex(2);
6005
6006 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bias);
6007
6008
6009 assertEquals(biasX, calibrator.getBiasX(), 0.0);
6010 assertEquals(biasY, calibrator.getBiasY(), 0.0);
6011 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6012 final var bx1 = calibrator.getBiasXAsAcceleration();
6013 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6014 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6015 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6016 calibrator.getBiasXAsAcceleration(bx2);
6017 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6018 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6019 final var by1 = calibrator.getBiasYAsAcceleration();
6020 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6021 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6022 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6023 calibrator.getBiasYAsAcceleration(by2);
6024 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6025 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6026 final var bz1 = calibrator.getBiasZAsAcceleration();
6027 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6028 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6029 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6030 calibrator.getBiasZAsAcceleration(bz2);
6031 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6032 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6033 final var biasTriad1 = calibrator.getBiasAsTriad();
6034 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6035 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6036 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6037 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6038 final var biasTriad2 = new AccelerationTriad();
6039 calibrator.getBiasAsTriad(biasTriad2);
6040 assertEquals(biasTriad1, biasTriad2);
6041 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6042 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6043 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6044 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6045 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6046 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6047 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6048 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6049 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6050 final var bias1 = calibrator.getBias();
6051 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6052 final var bias2 = new double[3];
6053 calibrator.getBias(bias2);
6054 assertArrayEquals(bias1, bias2, 0.0);
6055 final var b1 = calibrator.getBiasAsMatrix();
6056 assertEquals(b1, ba);
6057 final var b2 = new Matrix(3, 1);
6058 calibrator.getBiasAsMatrix(b2);
6059 assertEquals(b1, b2);
6060 final var ma1 = calibrator.getInitialMa();
6061 assertEquals(new Matrix(3, 3), ma1);
6062 final var ma2 = new Matrix(3, 3);
6063 calibrator.getInitialMa(ma2);
6064 assertEquals(ma1, ma2);
6065 assertNull(calibrator.getMeasurements());
6066 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6067 calibrator.getMeasurementType());
6068 assertFalse(calibrator.isOrderedMeasurementsRequired());
6069 assertFalse(calibrator.isQualityScoresRequired());
6070 assertTrue(calibrator.isCommonAxisUsed());
6071 assertNull(calibrator.getListener());
6072 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
6073 assertFalse(calibrator.isReady());
6074 assertFalse(calibrator.isRunning());
6075 assertNull(calibrator.getEstimatedMa());
6076 assertNull(calibrator.getEstimatedSx());
6077 assertNull(calibrator.getEstimatedSy());
6078 assertNull(calibrator.getEstimatedSz());
6079 assertNull(calibrator.getEstimatedMxy());
6080 assertNull(calibrator.getEstimatedMxz());
6081 assertNull(calibrator.getEstimatedMyx());
6082 assertNull(calibrator.getEstimatedMyz());
6083 assertNull(calibrator.getEstimatedMzx());
6084 assertNull(calibrator.getEstimatedMzy());
6085 assertNull(calibrator.getEstimatedCovariance());
6086 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6087 assertNull(calibrator.getGroundTruthGravityNorm());
6088 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6089 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6090 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6091
6092
6093 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(true,
6094 new double[1]));
6095 }
6096
6097 @Test
6098 void testConstructor60() throws WrongSizeException {
6099 final var ba = generateBa();
6100 final var bias = ba.getBuffer();
6101 final var biasX = ba.getElementAtIndex(0);
6102 final var biasY = ba.getElementAtIndex(1);
6103 final var biasZ = ba.getElementAtIndex(2);
6104
6105 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bias,
6106 this);
6107
6108
6109 assertEquals(biasX, calibrator.getBiasX(), 0.0);
6110 assertEquals(biasY, calibrator.getBiasY(), 0.0);
6111 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6112 final var bx1 = calibrator.getBiasXAsAcceleration();
6113 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6114 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6115 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6116 calibrator.getBiasXAsAcceleration(bx2);
6117 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6118 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6119 final var by1 = calibrator.getBiasYAsAcceleration();
6120 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6121 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6122 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6123 calibrator.getBiasYAsAcceleration(by2);
6124 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6125 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6126 final var bz1 = calibrator.getBiasZAsAcceleration();
6127 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6128 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6129 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6130 calibrator.getBiasZAsAcceleration(bz2);
6131 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6132 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6133 final var biasTriad1 = calibrator.getBiasAsTriad();
6134 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6135 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6136 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6137 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6138 final var biasTriad2 = new AccelerationTriad();
6139 calibrator.getBiasAsTriad(biasTriad2);
6140 assertEquals(biasTriad1, biasTriad2);
6141 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6142 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6143 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6144 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6145 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6146 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6147 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6148 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6149 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6150 final var bias1 = calibrator.getBias();
6151 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6152 final var bias2 = new double[3];
6153 calibrator.getBias(bias2);
6154 assertArrayEquals(bias1, bias2, 0.0);
6155 final var b1 = calibrator.getBiasAsMatrix();
6156 assertEquals(b1, ba);
6157 final var b2 = new Matrix(3, 1);
6158 calibrator.getBiasAsMatrix(b2);
6159 assertEquals(b1, b2);
6160 final var ma1 = calibrator.getInitialMa();
6161 assertEquals(new Matrix(3, 3), ma1);
6162 final var ma2 = new Matrix(3, 3);
6163 calibrator.getInitialMa(ma2);
6164 assertEquals(ma1, ma2);
6165 assertNull(calibrator.getMeasurements());
6166 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6167 calibrator.getMeasurementType());
6168 assertFalse(calibrator.isOrderedMeasurementsRequired());
6169 assertFalse(calibrator.isQualityScoresRequired());
6170 assertTrue(calibrator.isCommonAxisUsed());
6171 assertSame(this, calibrator.getListener());
6172 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
6173 assertFalse(calibrator.isReady());
6174 assertFalse(calibrator.isRunning());
6175 assertNull(calibrator.getEstimatedMa());
6176 assertNull(calibrator.getEstimatedSx());
6177 assertNull(calibrator.getEstimatedSy());
6178 assertNull(calibrator.getEstimatedSz());
6179 assertNull(calibrator.getEstimatedMxy());
6180 assertNull(calibrator.getEstimatedMxz());
6181 assertNull(calibrator.getEstimatedMyx());
6182 assertNull(calibrator.getEstimatedMyz());
6183 assertNull(calibrator.getEstimatedMzx());
6184 assertNull(calibrator.getEstimatedMzy());
6185 assertNull(calibrator.getEstimatedCovariance());
6186 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6187 assertNull(calibrator.getGroundTruthGravityNorm());
6188 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6189 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6190 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6191
6192
6193 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
6194 true, new double[1], this));
6195 }
6196
6197 @Test
6198 void testConstructor61() throws WrongSizeException {
6199 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
6200
6201 final var ba = generateBa();
6202 final var bias = ba.getBuffer();
6203 final var biasX = ba.getElementAtIndex(0);
6204 final var biasY = ba.getElementAtIndex(1);
6205 final var biasZ = ba.getElementAtIndex(2);
6206
6207 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
6208 bias);
6209
6210
6211 assertEquals(biasX, calibrator.getBiasX(), 0.0);
6212 assertEquals(biasY, calibrator.getBiasY(), 0.0);
6213 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6214 final var bx1 = calibrator.getBiasXAsAcceleration();
6215 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6216 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6217 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6218 calibrator.getBiasXAsAcceleration(bx2);
6219 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6220 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6221 final var by1 = calibrator.getBiasYAsAcceleration();
6222 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6223 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6224 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6225 calibrator.getBiasYAsAcceleration(by2);
6226 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6227 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6228 final var bz1 = calibrator.getBiasZAsAcceleration();
6229 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6230 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6231 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6232 calibrator.getBiasZAsAcceleration(bz2);
6233 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6234 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6235 final var biasTriad1 = calibrator.getBiasAsTriad();
6236 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6237 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6238 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6239 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6240 final var biasTriad2 = new AccelerationTriad();
6241 calibrator.getBiasAsTriad(biasTriad2);
6242 assertEquals(biasTriad1, biasTriad2);
6243 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6244 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6245 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6246 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6247 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6248 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6249 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6250 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6251 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6252 final var bias1 = calibrator.getBias();
6253 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6254 final var bias2 = new double[3];
6255 calibrator.getBias(bias2);
6256 assertArrayEquals(bias1, bias2, 0.0);
6257 final var b1 = calibrator.getBiasAsMatrix();
6258 assertEquals(b1, ba);
6259 final var b2 = new Matrix(3, 1);
6260 calibrator.getBiasAsMatrix(b2);
6261 assertEquals(b1, b2);
6262 final var ma1 = calibrator.getInitialMa();
6263 assertEquals(new Matrix(3, 3), ma1);
6264 final var ma2 = new Matrix(3, 3);
6265 calibrator.getInitialMa(ma2);
6266 assertEquals(ma1, ma2);
6267 assertSame(measurements, calibrator.getMeasurements());
6268 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6269 calibrator.getMeasurementType());
6270 assertFalse(calibrator.isOrderedMeasurementsRequired());
6271 assertFalse(calibrator.isQualityScoresRequired());
6272 assertTrue(calibrator.isCommonAxisUsed());
6273 assertNull(calibrator.getListener());
6274 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
6275 assertFalse(calibrator.isReady());
6276 assertFalse(calibrator.isRunning());
6277 assertNull(calibrator.getEstimatedMa());
6278 assertNull(calibrator.getEstimatedSx());
6279 assertNull(calibrator.getEstimatedSy());
6280 assertNull(calibrator.getEstimatedSz());
6281 assertNull(calibrator.getEstimatedMxy());
6282 assertNull(calibrator.getEstimatedMxz());
6283 assertNull(calibrator.getEstimatedMyx());
6284 assertNull(calibrator.getEstimatedMyz());
6285 assertNull(calibrator.getEstimatedMzx());
6286 assertNull(calibrator.getEstimatedMzy());
6287 assertNull(calibrator.getEstimatedCovariance());
6288 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6289 assertNull(calibrator.getGroundTruthGravityNorm());
6290 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6291 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6292 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6293
6294
6295 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
6296 measurements, true, new double[1]));
6297 }
6298
6299 @Test
6300 void testConstructor62() throws WrongSizeException {
6301 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
6302
6303 final var ba = generateBa();
6304 final var bias = ba.getBuffer();
6305 final var biasX = ba.getElementAtIndex(0);
6306 final var biasY = ba.getElementAtIndex(1);
6307 final var biasZ = ba.getElementAtIndex(2);
6308
6309 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
6310 bias, this);
6311
6312
6313 assertEquals(biasX, calibrator.getBiasX(), 0.0);
6314 assertEquals(biasY, calibrator.getBiasY(), 0.0);
6315 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6316 final var bx1 = calibrator.getBiasXAsAcceleration();
6317 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6318 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6319 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6320 calibrator.getBiasXAsAcceleration(bx2);
6321 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6322 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6323 final var by1 = calibrator.getBiasYAsAcceleration();
6324 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6325 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6326 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6327 calibrator.getBiasYAsAcceleration(by2);
6328 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6329 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6330 final var bz1 = calibrator.getBiasZAsAcceleration();
6331 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6332 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6333 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6334 calibrator.getBiasZAsAcceleration(bz2);
6335 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6336 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6337 final var biasTriad1 = calibrator.getBiasAsTriad();
6338 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6339 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6340 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6341 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6342 final var biasTriad2 = new AccelerationTriad();
6343 calibrator.getBiasAsTriad(biasTriad2);
6344 assertEquals(biasTriad1, biasTriad2);
6345 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6346 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6347 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6348 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6349 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6350 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6351 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6352 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6353 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6354 final var bias1 = calibrator.getBias();
6355 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6356 final var bias2 = new double[3];
6357 calibrator.getBias(bias2);
6358 assertArrayEquals(bias1, bias2, 0.0);
6359 final var b1 = calibrator.getBiasAsMatrix();
6360 assertEquals(b1, ba);
6361 final var b2 = new Matrix(3, 1);
6362 calibrator.getBiasAsMatrix(b2);
6363 assertEquals(b1, b2);
6364 final var ma1 = calibrator.getInitialMa();
6365 assertEquals(new Matrix(3, 3), ma1);
6366 final var ma2 = new Matrix(3, 3);
6367 calibrator.getInitialMa(ma2);
6368 assertEquals(ma1, ma2);
6369 assertSame(measurements, calibrator.getMeasurements());
6370 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6371 calibrator.getMeasurementType());
6372 assertFalse(calibrator.isOrderedMeasurementsRequired());
6373 assertFalse(calibrator.isQualityScoresRequired());
6374 assertTrue(calibrator.isCommonAxisUsed());
6375 assertSame(this, calibrator.getListener());
6376 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
6377 assertFalse(calibrator.isReady());
6378 assertFalse(calibrator.isRunning());
6379 assertNull(calibrator.getEstimatedMa());
6380 assertNull(calibrator.getEstimatedSx());
6381 assertNull(calibrator.getEstimatedSy());
6382 assertNull(calibrator.getEstimatedSz());
6383 assertNull(calibrator.getEstimatedMxy());
6384 assertNull(calibrator.getEstimatedMxz());
6385 assertNull(calibrator.getEstimatedMyx());
6386 assertNull(calibrator.getEstimatedMyz());
6387 assertNull(calibrator.getEstimatedMzx());
6388 assertNull(calibrator.getEstimatedMzy());
6389 assertNull(calibrator.getEstimatedCovariance());
6390 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6391 assertNull(calibrator.getGroundTruthGravityNorm());
6392 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6393 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6394 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6395
6396
6397 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
6398 measurements, true, new double[1], this));
6399 }
6400
6401 @Test
6402 void testConstructor63() throws WrongSizeException {
6403 final var ba = generateBa();
6404 final var biasX = ba.getElementAtIndex(0);
6405 final var biasY = ba.getElementAtIndex(1);
6406 final var biasZ = ba.getElementAtIndex(2);
6407
6408 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba);
6409
6410
6411 assertEquals(biasX, calibrator.getBiasX(), 0.0);
6412 assertEquals(biasY, calibrator.getBiasY(), 0.0);
6413 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6414 final var bx1 = calibrator.getBiasXAsAcceleration();
6415 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6416 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6417 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6418 calibrator.getBiasXAsAcceleration(bx2);
6419 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6420 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6421 final var by1 = calibrator.getBiasYAsAcceleration();
6422 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6423 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6424 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6425 calibrator.getBiasYAsAcceleration(by2);
6426 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6427 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6428 final var bz1 = calibrator.getBiasZAsAcceleration();
6429 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6430 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6431 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6432 calibrator.getBiasZAsAcceleration(bz2);
6433 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6434 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6435 final var biasTriad1 = calibrator.getBiasAsTriad();
6436 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6437 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6438 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6439 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6440 final var biasTriad2 = new AccelerationTriad();
6441 calibrator.getBiasAsTriad(biasTriad2);
6442 assertEquals(biasTriad1, biasTriad2);
6443 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6444 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6445 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6446 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6447 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6448 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6449 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6450 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6451 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6452 final var bias1 = calibrator.getBias();
6453 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6454 final var bias2 = new double[3];
6455 calibrator.getBias(bias2);
6456 assertArrayEquals(bias1, bias2, 0.0);
6457 final var b1 = calibrator.getBiasAsMatrix();
6458 assertEquals(b1, ba);
6459 final var b2 = new Matrix(3, 1);
6460 calibrator.getBiasAsMatrix(b2);
6461 assertEquals(b1, b2);
6462 final var ma1 = calibrator.getInitialMa();
6463 assertEquals(new Matrix(3, 3), ma1);
6464 final var ma2 = new Matrix(3, 3);
6465 calibrator.getInitialMa(ma2);
6466 assertEquals(ma1, ma2);
6467 assertNull(calibrator.getMeasurements());
6468 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6469 calibrator.getMeasurementType());
6470 assertFalse(calibrator.isOrderedMeasurementsRequired());
6471 assertFalse(calibrator.isQualityScoresRequired());
6472 assertFalse(calibrator.isCommonAxisUsed());
6473 assertNull(calibrator.getListener());
6474 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
6475 assertFalse(calibrator.isReady());
6476 assertFalse(calibrator.isRunning());
6477 assertNull(calibrator.getEstimatedMa());
6478 assertNull(calibrator.getEstimatedSx());
6479 assertNull(calibrator.getEstimatedSy());
6480 assertNull(calibrator.getEstimatedSz());
6481 assertNull(calibrator.getEstimatedMxy());
6482 assertNull(calibrator.getEstimatedMxz());
6483 assertNull(calibrator.getEstimatedMyx());
6484 assertNull(calibrator.getEstimatedMyz());
6485 assertNull(calibrator.getEstimatedMzx());
6486 assertNull(calibrator.getEstimatedMzy());
6487 assertNull(calibrator.getEstimatedCovariance());
6488 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6489 assertNull(calibrator.getGroundTruthGravityNorm());
6490 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6491 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6492 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6493
6494
6495 final var m1 = new Matrix(1, 1);
6496 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(m1));
6497 final var m2 = new Matrix(1, 3);
6498 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(m2));
6499 }
6500
6501 @Test
6502 void testConstructor64() throws WrongSizeException {
6503 final var ba = generateBa();
6504 final var biasX = ba.getElementAtIndex(0);
6505 final var biasY = ba.getElementAtIndex(1);
6506 final var biasZ = ba.getElementAtIndex(2);
6507
6508 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba, this);
6509
6510
6511 assertEquals(biasX, calibrator.getBiasX(), 0.0);
6512 assertEquals(biasY, calibrator.getBiasY(), 0.0);
6513 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6514 final var bx1 = calibrator.getBiasXAsAcceleration();
6515 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6516 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6517 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6518 calibrator.getBiasXAsAcceleration(bx2);
6519 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6520 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6521 final var by1 = calibrator.getBiasYAsAcceleration();
6522 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6523 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6524 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6525 calibrator.getBiasYAsAcceleration(by2);
6526 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6527 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6528 final var bz1 = calibrator.getBiasZAsAcceleration();
6529 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6530 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6531 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6532 calibrator.getBiasZAsAcceleration(bz2);
6533 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6534 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6535 final var biasTriad1 = calibrator.getBiasAsTriad();
6536 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6537 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6538 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6539 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6540 final var biasTriad2 = new AccelerationTriad();
6541 calibrator.getBiasAsTriad(biasTriad2);
6542 assertEquals(biasTriad1, biasTriad2);
6543 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6544 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6545 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6546 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6547 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6548 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6549 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6550 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6551 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6552 final var bias1 = calibrator.getBias();
6553 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6554 final var bias2 = new double[3];
6555 calibrator.getBias(bias2);
6556 assertArrayEquals(bias1, bias2, 0.0);
6557 final var b1 = calibrator.getBiasAsMatrix();
6558 assertEquals(b1, ba);
6559 final var b2 = new Matrix(3, 1);
6560 calibrator.getBiasAsMatrix(b2);
6561 assertEquals(b1, b2);
6562 final var ma1 = calibrator.getInitialMa();
6563 assertEquals(new Matrix(3, 3), ma1);
6564 final var ma2 = new Matrix(3, 3);
6565 calibrator.getInitialMa(ma2);
6566 assertEquals(ma1, ma2);
6567 assertNull(calibrator.getMeasurements());
6568 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6569 calibrator.getMeasurementType());
6570 assertFalse(calibrator.isOrderedMeasurementsRequired());
6571 assertFalse(calibrator.isQualityScoresRequired());
6572 assertFalse(calibrator.isCommonAxisUsed());
6573 assertSame(this, calibrator.getListener());
6574 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
6575 assertFalse(calibrator.isReady());
6576 assertFalse(calibrator.isRunning());
6577 assertNull(calibrator.getEstimatedMa());
6578 assertNull(calibrator.getEstimatedSx());
6579 assertNull(calibrator.getEstimatedSy());
6580 assertNull(calibrator.getEstimatedSz());
6581 assertNull(calibrator.getEstimatedMxy());
6582 assertNull(calibrator.getEstimatedMxz());
6583 assertNull(calibrator.getEstimatedMyx());
6584 assertNull(calibrator.getEstimatedMyz());
6585 assertNull(calibrator.getEstimatedMzx());
6586 assertNull(calibrator.getEstimatedMzy());
6587 assertNull(calibrator.getEstimatedCovariance());
6588 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6589 assertNull(calibrator.getGroundTruthGravityNorm());
6590 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6591 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6592 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6593
6594
6595 final var m1 = new Matrix(1, 1);
6596 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(m1));
6597 final var m2 = new Matrix(1, 3);
6598 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(m2));
6599 }
6600
6601 @Test
6602 void testConstructor65() throws WrongSizeException {
6603 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
6604
6605 final var ba = generateBa();
6606 final var biasX = ba.getElementAtIndex(0);
6607 final var biasY = ba.getElementAtIndex(1);
6608 final var biasZ = ba.getElementAtIndex(2);
6609
6610 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, ba);
6611
6612
6613 assertEquals(biasX, calibrator.getBiasX(), 0.0);
6614 assertEquals(biasY, calibrator.getBiasY(), 0.0);
6615 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6616 final var bx1 = calibrator.getBiasXAsAcceleration();
6617 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6618 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6619 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6620 calibrator.getBiasXAsAcceleration(bx2);
6621 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6622 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6623 final var by1 = calibrator.getBiasYAsAcceleration();
6624 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6625 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6626 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6627 calibrator.getBiasYAsAcceleration(by2);
6628 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6629 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6630 final var bz1 = calibrator.getBiasZAsAcceleration();
6631 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6632 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6633 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6634 calibrator.getBiasZAsAcceleration(bz2);
6635 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6636 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6637 final var biasTriad1 = calibrator.getBiasAsTriad();
6638 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6639 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6640 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6641 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6642 final var biasTriad2 = new AccelerationTriad();
6643 calibrator.getBiasAsTriad(biasTriad2);
6644 assertEquals(biasTriad1, biasTriad2);
6645 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6646 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6647 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6648 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6649 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6650 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6651 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6652 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6653 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6654 final var bias1 = calibrator.getBias();
6655 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6656 final var bias2 = new double[3];
6657 calibrator.getBias(bias2);
6658 assertArrayEquals(bias1, bias2, 0.0);
6659 final var b1 = calibrator.getBiasAsMatrix();
6660 assertEquals(b1, ba);
6661 final var b2 = new Matrix(3, 1);
6662 calibrator.getBiasAsMatrix(b2);
6663 assertEquals(b1, b2);
6664 final var ma1 = calibrator.getInitialMa();
6665 assertEquals(new Matrix(3, 3), ma1);
6666 final var ma2 = new Matrix(3, 3);
6667 calibrator.getInitialMa(ma2);
6668 assertEquals(ma1, ma2);
6669 assertSame(measurements, calibrator.getMeasurements());
6670 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6671 calibrator.getMeasurementType());
6672 assertFalse(calibrator.isOrderedMeasurementsRequired());
6673 assertFalse(calibrator.isQualityScoresRequired());
6674 assertFalse(calibrator.isCommonAxisUsed());
6675 assertNull(calibrator.getListener());
6676 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
6677 assertFalse(calibrator.isReady());
6678 assertFalse(calibrator.isRunning());
6679 assertNull(calibrator.getEstimatedMa());
6680 assertNull(calibrator.getEstimatedSx());
6681 assertNull(calibrator.getEstimatedSy());
6682 assertNull(calibrator.getEstimatedSz());
6683 assertNull(calibrator.getEstimatedMxy());
6684 assertNull(calibrator.getEstimatedMxz());
6685 assertNull(calibrator.getEstimatedMyx());
6686 assertNull(calibrator.getEstimatedMyz());
6687 assertNull(calibrator.getEstimatedMzx());
6688 assertNull(calibrator.getEstimatedMzy());
6689 assertNull(calibrator.getEstimatedCovariance());
6690 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6691 assertNull(calibrator.getGroundTruthGravityNorm());
6692 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6693 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6694 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6695
6696
6697 final var m1 = new Matrix(1, 1);
6698 assertThrows(IllegalArgumentException.class,
6699 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, m1));
6700 final var m2 = new Matrix(1, 3);
6701 assertThrows(IllegalArgumentException.class,
6702 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, m2));
6703 }
6704
6705 @Test
6706 void testConstructor66() throws WrongSizeException {
6707 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
6708
6709 final var ba = generateBa();
6710 final var biasX = ba.getElementAtIndex(0);
6711 final var biasY = ba.getElementAtIndex(1);
6712 final var biasZ = ba.getElementAtIndex(2);
6713
6714 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, ba, this);
6715
6716
6717 assertEquals(biasX, calibrator.getBiasX(), 0.0);
6718 assertEquals(biasY, calibrator.getBiasY(), 0.0);
6719 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6720 final var bx1 = calibrator.getBiasXAsAcceleration();
6721 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6722 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6723 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6724 calibrator.getBiasXAsAcceleration(bx2);
6725 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6726 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6727 final var by1 = calibrator.getBiasYAsAcceleration();
6728 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6729 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6730 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6731 calibrator.getBiasYAsAcceleration(by2);
6732 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6733 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6734 final var bz1 = calibrator.getBiasZAsAcceleration();
6735 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6736 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6737 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6738 calibrator.getBiasZAsAcceleration(bz2);
6739 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6740 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6741 final var biasTriad1 = calibrator.getBiasAsTriad();
6742 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6743 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6744 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6745 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6746 final var biasTriad2 = new AccelerationTriad();
6747 calibrator.getBiasAsTriad(biasTriad2);
6748 assertEquals(biasTriad1, biasTriad2);
6749 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6750 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6751 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6752 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6753 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6754 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6755 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6756 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6757 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6758 final var bias1 = calibrator.getBias();
6759 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6760 final var bias2 = new double[3];
6761 calibrator.getBias(bias2);
6762 assertArrayEquals(bias1, bias2, 0.0);
6763 final var b1 = calibrator.getBiasAsMatrix();
6764 assertEquals(b1, ba);
6765 final var b2 = new Matrix(3, 1);
6766 calibrator.getBiasAsMatrix(b2);
6767 assertEquals(b1, b2);
6768 final var ma1 = calibrator.getInitialMa();
6769 assertEquals(new Matrix(3, 3), ma1);
6770 final var ma2 = new Matrix(3, 3);
6771 calibrator.getInitialMa(ma2);
6772 assertEquals(ma1, ma2);
6773 assertSame(measurements, calibrator.getMeasurements());
6774 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6775 calibrator.getMeasurementType());
6776 assertFalse(calibrator.isOrderedMeasurementsRequired());
6777 assertFalse(calibrator.isQualityScoresRequired());
6778 assertFalse(calibrator.isCommonAxisUsed());
6779 assertSame(this, calibrator.getListener());
6780 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
6781 assertFalse(calibrator.isReady());
6782 assertFalse(calibrator.isRunning());
6783 assertNull(calibrator.getEstimatedMa());
6784 assertNull(calibrator.getEstimatedSx());
6785 assertNull(calibrator.getEstimatedSy());
6786 assertNull(calibrator.getEstimatedSz());
6787 assertNull(calibrator.getEstimatedMxy());
6788 assertNull(calibrator.getEstimatedMxz());
6789 assertNull(calibrator.getEstimatedMyx());
6790 assertNull(calibrator.getEstimatedMyz());
6791 assertNull(calibrator.getEstimatedMzx());
6792 assertNull(calibrator.getEstimatedMzy());
6793 assertNull(calibrator.getEstimatedCovariance());
6794 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6795 assertNull(calibrator.getGroundTruthGravityNorm());
6796 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6797 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6798 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6799
6800
6801 final var m1 = new Matrix(1, 1);
6802 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
6803 measurements, m1, this));
6804 final var m2 = new Matrix(1, 3);
6805 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
6806 measurements, m2, this));
6807 }
6808
6809 @Test
6810 void testConstructor67() throws WrongSizeException {
6811 final var ba = generateBa();
6812 final var biasX = ba.getElementAtIndex(0);
6813 final var biasY = ba.getElementAtIndex(1);
6814 final var biasZ = ba.getElementAtIndex(2);
6815
6816 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, ba);
6817
6818
6819 assertEquals(biasX, calibrator.getBiasX(), 0.0);
6820 assertEquals(biasY, calibrator.getBiasY(), 0.0);
6821 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6822 final var bx1 = calibrator.getBiasXAsAcceleration();
6823 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6824 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6825 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6826 calibrator.getBiasXAsAcceleration(bx2);
6827 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6828 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6829 final var by1 = calibrator.getBiasYAsAcceleration();
6830 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6831 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6832 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6833 calibrator.getBiasYAsAcceleration(by2);
6834 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6835 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6836 final var bz1 = calibrator.getBiasZAsAcceleration();
6837 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6838 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6839 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6840 calibrator.getBiasZAsAcceleration(bz2);
6841 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6842 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6843 final var biasTriad1 = calibrator.getBiasAsTriad();
6844 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6845 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6846 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6847 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6848 final var biasTriad2 = new AccelerationTriad();
6849 calibrator.getBiasAsTriad(biasTriad2);
6850 assertEquals(biasTriad1, biasTriad2);
6851 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6852 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6853 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6854 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6855 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6856 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6857 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6858 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6859 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6860 final var bias1 = calibrator.getBias();
6861 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6862 final var bias2 = new double[3];
6863 calibrator.getBias(bias2);
6864 assertArrayEquals(bias1, bias2, 0.0);
6865 final var b1 = calibrator.getBiasAsMatrix();
6866 assertEquals(b1, ba);
6867 final var b2 = new Matrix(3, 1);
6868 calibrator.getBiasAsMatrix(b2);
6869 assertEquals(b1, b2);
6870 final var ma1 = calibrator.getInitialMa();
6871 assertEquals(new Matrix(3, 3), ma1);
6872 final var ma2 = new Matrix(3, 3);
6873 calibrator.getInitialMa(ma2);
6874 assertEquals(ma1, ma2);
6875 assertNull(calibrator.getMeasurements());
6876 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6877 calibrator.getMeasurementType());
6878 assertFalse(calibrator.isOrderedMeasurementsRequired());
6879 assertFalse(calibrator.isQualityScoresRequired());
6880 assertTrue(calibrator.isCommonAxisUsed());
6881 assertNull(calibrator.getListener());
6882 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
6883 assertFalse(calibrator.isReady());
6884 assertFalse(calibrator.isRunning());
6885 assertNull(calibrator.getEstimatedMa());
6886 assertNull(calibrator.getEstimatedSx());
6887 assertNull(calibrator.getEstimatedSy());
6888 assertNull(calibrator.getEstimatedSz());
6889 assertNull(calibrator.getEstimatedMxy());
6890 assertNull(calibrator.getEstimatedMxz());
6891 assertNull(calibrator.getEstimatedMyx());
6892 assertNull(calibrator.getEstimatedMyz());
6893 assertNull(calibrator.getEstimatedMzx());
6894 assertNull(calibrator.getEstimatedMzy());
6895 assertNull(calibrator.getEstimatedCovariance());
6896 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6897 assertNull(calibrator.getGroundTruthGravityNorm());
6898 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6899 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6900 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6901
6902
6903 final var m1 = new Matrix(1, 1);
6904 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
6905 true, m1));
6906 final var m2 = new Matrix(1, 3);
6907 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
6908 true, m2));
6909 }
6910
6911 @Test
6912 void testConstructor68() throws WrongSizeException {
6913 final var ba = generateBa();
6914 final var biasX = ba.getElementAtIndex(0);
6915 final var biasY = ba.getElementAtIndex(1);
6916 final var biasZ = ba.getElementAtIndex(2);
6917
6918 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, ba,
6919 this);
6920
6921
6922 assertEquals(biasX, calibrator.getBiasX(), 0.0);
6923 assertEquals(biasY, calibrator.getBiasY(), 0.0);
6924 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6925 final var bx1 = calibrator.getBiasXAsAcceleration();
6926 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6927 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6928 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6929 calibrator.getBiasXAsAcceleration(bx2);
6930 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6931 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6932 final var by1 = calibrator.getBiasYAsAcceleration();
6933 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6934 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6935 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6936 calibrator.getBiasYAsAcceleration(by2);
6937 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6938 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6939 final var bz1 = calibrator.getBiasZAsAcceleration();
6940 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6941 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6942 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6943 calibrator.getBiasZAsAcceleration(bz2);
6944 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6945 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6946 final var biasTriad1 = calibrator.getBiasAsTriad();
6947 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6948 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6949 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6950 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6951 final var biasTriad2 = new AccelerationTriad();
6952 calibrator.getBiasAsTriad(biasTriad2);
6953 assertEquals(biasTriad1, biasTriad2);
6954 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6955 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6956 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6957 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6958 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6959 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6960 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6961 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6962 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6963 final var bias1 = calibrator.getBias();
6964 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6965 final var bias2 = new double[3];
6966 calibrator.getBias(bias2);
6967 assertArrayEquals(bias1, bias2, 0.0);
6968 final var b1 = calibrator.getBiasAsMatrix();
6969 assertEquals(b1, ba);
6970 final var b2 = new Matrix(3, 1);
6971 calibrator.getBiasAsMatrix(b2);
6972 assertEquals(b1, b2);
6973 final var ma1 = calibrator.getInitialMa();
6974 assertEquals(new Matrix(3, 3), ma1);
6975 final var ma2 = new Matrix(3, 3);
6976 calibrator.getInitialMa(ma2);
6977 assertEquals(ma1, ma2);
6978 assertNull(calibrator.getMeasurements());
6979 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6980 calibrator.getMeasurementType());
6981 assertFalse(calibrator.isOrderedMeasurementsRequired());
6982 assertFalse(calibrator.isQualityScoresRequired());
6983 assertTrue(calibrator.isCommonAxisUsed());
6984 assertSame(this, calibrator.getListener());
6985 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
6986 assertFalse(calibrator.isReady());
6987 assertFalse(calibrator.isRunning());
6988 assertNull(calibrator.getEstimatedMa());
6989 assertNull(calibrator.getEstimatedSx());
6990 assertNull(calibrator.getEstimatedSy());
6991 assertNull(calibrator.getEstimatedSz());
6992 assertNull(calibrator.getEstimatedMxy());
6993 assertNull(calibrator.getEstimatedMxz());
6994 assertNull(calibrator.getEstimatedMyx());
6995 assertNull(calibrator.getEstimatedMyz());
6996 assertNull(calibrator.getEstimatedMzx());
6997 assertNull(calibrator.getEstimatedMzy());
6998 assertNull(calibrator.getEstimatedCovariance());
6999 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7000 assertNull(calibrator.getGroundTruthGravityNorm());
7001 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7002 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7003 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7004
7005
7006 final var m1 = new Matrix(1, 1);
7007 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7008 true, m1, this));
7009 final var m2 = new Matrix(1, 3);
7010 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7011 true, m2, this));
7012 }
7013
7014 @Test
7015 void testConstructor69() throws WrongSizeException {
7016 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
7017
7018 final var ba = generateBa();
7019 final var biasX = ba.getElementAtIndex(0);
7020 final var biasY = ba.getElementAtIndex(1);
7021 final var biasZ = ba.getElementAtIndex(2);
7022
7023 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
7024 ba);
7025
7026
7027 assertEquals(biasX, calibrator.getBiasX(), 0.0);
7028 assertEquals(biasY, calibrator.getBiasY(), 0.0);
7029 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7030 final var bx1 = calibrator.getBiasXAsAcceleration();
7031 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7032 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7033 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7034 calibrator.getBiasXAsAcceleration(bx2);
7035 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7036 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7037 final var by1 = calibrator.getBiasYAsAcceleration();
7038 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7039 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7040 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7041 calibrator.getBiasYAsAcceleration(by2);
7042 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7043 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7044 final var bz1 = calibrator.getBiasZAsAcceleration();
7045 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7046 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7047 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7048 calibrator.getBiasZAsAcceleration(bz2);
7049 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7050 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7051 final var biasTriad1 = calibrator.getBiasAsTriad();
7052 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7053 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7054 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7055 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7056 final var biasTriad2 = new AccelerationTriad();
7057 calibrator.getBiasAsTriad(biasTriad2);
7058 assertEquals(biasTriad1, biasTriad2);
7059 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
7060 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
7061 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
7062 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
7063 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
7064 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
7065 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
7066 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
7067 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
7068 final var bias1 = calibrator.getBias();
7069 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7070 final var bias2 = new double[3];
7071 calibrator.getBias(bias2);
7072 assertArrayEquals(bias1, bias2, 0.0);
7073 final var b1 = calibrator.getBiasAsMatrix();
7074 assertEquals(b1, ba);
7075 final var b2 = new Matrix(3, 1);
7076 calibrator.getBiasAsMatrix(b2);
7077 assertEquals(b1, b2);
7078 final var ma1 = calibrator.getInitialMa();
7079 assertEquals(new Matrix(3, 3), ma1);
7080 final var ma2 = new Matrix(3, 3);
7081 calibrator.getInitialMa(ma2);
7082 assertEquals(ma1, ma2);
7083 assertSame(measurements, calibrator.getMeasurements());
7084 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7085 calibrator.getMeasurementType());
7086 assertFalse(calibrator.isOrderedMeasurementsRequired());
7087 assertFalse(calibrator.isQualityScoresRequired());
7088 assertTrue(calibrator.isCommonAxisUsed());
7089 assertNull(calibrator.getListener());
7090 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
7091 assertFalse(calibrator.isReady());
7092 assertFalse(calibrator.isRunning());
7093 assertNull(calibrator.getEstimatedMa());
7094 assertNull(calibrator.getEstimatedSx());
7095 assertNull(calibrator.getEstimatedSy());
7096 assertNull(calibrator.getEstimatedSz());
7097 assertNull(calibrator.getEstimatedMxy());
7098 assertNull(calibrator.getEstimatedMxz());
7099 assertNull(calibrator.getEstimatedMyx());
7100 assertNull(calibrator.getEstimatedMyz());
7101 assertNull(calibrator.getEstimatedMzx());
7102 assertNull(calibrator.getEstimatedMzy());
7103 assertNull(calibrator.getEstimatedCovariance());
7104 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7105 assertNull(calibrator.getGroundTruthGravityNorm());
7106 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7107 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7108 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7109
7110
7111 final var m1 = new Matrix(1, 1);
7112 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7113 measurements, true, m1));
7114 final var m2 = new Matrix(1, 3);
7115 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7116 measurements, true, m2));
7117 }
7118
7119 @Test
7120 void testConstructor70() throws WrongSizeException {
7121 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
7122
7123 final var ba = generateBa();
7124 final var biasX = ba.getElementAtIndex(0);
7125 final var biasY = ba.getElementAtIndex(1);
7126 final var biasZ = ba.getElementAtIndex(2);
7127
7128 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
7129 ba, this);
7130
7131
7132 assertEquals(biasX, calibrator.getBiasX(), 0.0);
7133 assertEquals(biasY, calibrator.getBiasY(), 0.0);
7134 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7135 final var bx1 = calibrator.getBiasXAsAcceleration();
7136 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7137 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7138 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7139 calibrator.getBiasXAsAcceleration(bx2);
7140 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7141 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7142 final var by1 = calibrator.getBiasYAsAcceleration();
7143 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7144 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7145 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7146 calibrator.getBiasYAsAcceleration(by2);
7147 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7148 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7149 final var bz1 = calibrator.getBiasZAsAcceleration();
7150 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7151 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7152 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7153 calibrator.getBiasZAsAcceleration(bz2);
7154 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7155 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7156 final var biasTriad1 = calibrator.getBiasAsTriad();
7157 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7158 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7159 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7160 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7161 final var biasTriad2 = new AccelerationTriad();
7162 calibrator.getBiasAsTriad(biasTriad2);
7163 assertEquals(biasTriad1, biasTriad2);
7164 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
7165 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
7166 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
7167 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
7168 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
7169 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
7170 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
7171 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
7172 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
7173 final var bias1 = calibrator.getBias();
7174 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7175 final var bias2 = new double[3];
7176 calibrator.getBias(bias2);
7177 assertArrayEquals(bias1, bias2, 0.0);
7178 final var b1 = calibrator.getBiasAsMatrix();
7179 assertEquals(b1, ba);
7180 final var b2 = new Matrix(3, 1);
7181 calibrator.getBiasAsMatrix(b2);
7182 assertEquals(b1, b2);
7183 final var ma1 = calibrator.getInitialMa();
7184 assertEquals(new Matrix(3, 3), ma1);
7185 final var ma2 = new Matrix(3, 3);
7186 calibrator.getInitialMa(ma2);
7187 assertEquals(ma1, ma2);
7188 assertSame(measurements, calibrator.getMeasurements());
7189 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7190 calibrator.getMeasurementType());
7191 assertFalse(calibrator.isOrderedMeasurementsRequired());
7192 assertFalse(calibrator.isQualityScoresRequired());
7193 assertTrue(calibrator.isCommonAxisUsed());
7194 assertSame(this, calibrator.getListener());
7195 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
7196 assertFalse(calibrator.isReady());
7197 assertFalse(calibrator.isRunning());
7198 assertNull(calibrator.getEstimatedMa());
7199 assertNull(calibrator.getEstimatedSx());
7200 assertNull(calibrator.getEstimatedSy());
7201 assertNull(calibrator.getEstimatedSz());
7202 assertNull(calibrator.getEstimatedMxy());
7203 assertNull(calibrator.getEstimatedMxz());
7204 assertNull(calibrator.getEstimatedMyx());
7205 assertNull(calibrator.getEstimatedMyz());
7206 assertNull(calibrator.getEstimatedMzx());
7207 assertNull(calibrator.getEstimatedMzy());
7208 assertNull(calibrator.getEstimatedCovariance());
7209 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7210 assertNull(calibrator.getGroundTruthGravityNorm());
7211 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7212 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7213 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7214
7215
7216 final var m1 = new Matrix(1, 1);
7217 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7218 measurements, true, m1, this));
7219 final var m2 = new Matrix(1, 3);
7220 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7221 measurements, true, m2, this));
7222 }
7223
7224 @Test
7225 void testConstructor71() throws WrongSizeException {
7226 final var ba = generateBa();
7227 final var biasX = ba.getElementAtIndex(0);
7228 final var biasY = ba.getElementAtIndex(1);
7229 final var biasZ = ba.getElementAtIndex(2);
7230
7231 final var ma = generateMaCommonAxis();
7232 final var sx = ma.getElementAt(0, 0);
7233 final var sy = ma.getElementAt(1, 1);
7234 final var sz = ma.getElementAt(2, 2);
7235 final var mxy = ma.getElementAt(0, 1);
7236 final var mxz = ma.getElementAt(0, 2);
7237 final var myx = ma.getElementAt(1, 0);
7238 final var myz = ma.getElementAt(1, 2);
7239 final var mzx = ma.getElementAt(2, 0);
7240 final var mzy = ma.getElementAt(2, 1);
7241
7242 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba, ma);
7243
7244
7245 assertEquals(biasX, calibrator.getBiasX(), 0.0);
7246 assertEquals(biasY, calibrator.getBiasY(), 0.0);
7247 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7248 final var bx1 = calibrator.getBiasXAsAcceleration();
7249 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7250 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7251 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7252 calibrator.getBiasXAsAcceleration(bx2);
7253 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7254 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7255 final var by1 = calibrator.getBiasYAsAcceleration();
7256 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7257 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7258 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7259 calibrator.getBiasYAsAcceleration(by2);
7260 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7261 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7262 final var bz1 = calibrator.getBiasZAsAcceleration();
7263 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7264 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7265 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7266 calibrator.getBiasZAsAcceleration(bz2);
7267 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7268 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7269 final var biasTriad1 = calibrator.getBiasAsTriad();
7270 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7271 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7272 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7273 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7274 final var biasTriad2 = new AccelerationTriad();
7275 calibrator.getBiasAsTriad(biasTriad2);
7276 assertEquals(biasTriad1, biasTriad2);
7277 assertEquals(sx, calibrator.getInitialSx(), 0.0);
7278 assertEquals(sy, calibrator.getInitialSy(), 0.0);
7279 assertEquals(sz, calibrator.getInitialSz(), 0.0);
7280 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
7281 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
7282 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
7283 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
7284 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
7285 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
7286 final var bias1 = calibrator.getBias();
7287 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7288 final var bias2 = new double[3];
7289 calibrator.getBias(bias2);
7290 assertArrayEquals(bias1, bias2, 0.0);
7291 final var b1 = calibrator.getBiasAsMatrix();
7292 assertEquals(b1, ba);
7293 final var b2 = new Matrix(3, 1);
7294 calibrator.getBiasAsMatrix(b2);
7295 assertEquals(b1, b2);
7296 final var ma1 = new Matrix(3, 3);
7297 ma1.setSubmatrix(0, 0, 2, 2,
7298 new double[]{sx, myx, mzx,
7299 mxy, sy, mzy,
7300 mxz, myz, sz});
7301 assertEquals(calibrator.getInitialMa(), ma1);
7302 final var ma2 = new Matrix(3, 3);
7303 calibrator.getInitialMa(ma2);
7304 assertEquals(ma1, ma2);
7305 assertNull(calibrator.getMeasurements());
7306 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7307 calibrator.getMeasurementType());
7308 assertFalse(calibrator.isOrderedMeasurementsRequired());
7309 assertFalse(calibrator.isQualityScoresRequired());
7310 assertFalse(calibrator.isCommonAxisUsed());
7311 assertNull(calibrator.getListener());
7312 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
7313 assertFalse(calibrator.isReady());
7314 assertFalse(calibrator.isRunning());
7315 assertNull(calibrator.getEstimatedMa());
7316 assertNull(calibrator.getEstimatedSx());
7317 assertNull(calibrator.getEstimatedSy());
7318 assertNull(calibrator.getEstimatedSz());
7319 assertNull(calibrator.getEstimatedMxy());
7320 assertNull(calibrator.getEstimatedMxz());
7321 assertNull(calibrator.getEstimatedMyx());
7322 assertNull(calibrator.getEstimatedMyz());
7323 assertNull(calibrator.getEstimatedMzx());
7324 assertNull(calibrator.getEstimatedMzy());
7325 assertNull(calibrator.getEstimatedCovariance());
7326 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7327 assertNull(calibrator.getGroundTruthGravityNorm());
7328 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7329 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7330 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7331
7332
7333 final var m1 = new Matrix(1, 1);
7334 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(m1, ma));
7335 final var m2 = new Matrix(1, 3);
7336 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(m2, ma));
7337 final var m3 = new Matrix(1, 3);
7338 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(ba, m3));
7339 final var m4 = new Matrix(3, 1);
7340 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(ba, m4));
7341 }
7342
7343 @Test
7344 void testConstructor72() throws WrongSizeException {
7345 final var ba = generateBa();
7346 final var biasX = ba.getElementAtIndex(0);
7347 final var biasY = ba.getElementAtIndex(1);
7348 final var biasZ = ba.getElementAtIndex(2);
7349
7350 final var ma = generateMaCommonAxis();
7351 final var sx = ma.getElementAt(0, 0);
7352 final var sy = ma.getElementAt(1, 1);
7353 final var sz = ma.getElementAt(2, 2);
7354 final var mxy = ma.getElementAt(0, 1);
7355 final var mxz = ma.getElementAt(0, 2);
7356 final var myx = ma.getElementAt(1, 0);
7357 final var myz = ma.getElementAt(1, 2);
7358 final var mzx = ma.getElementAt(2, 0);
7359 final var mzy = ma.getElementAt(2, 1);
7360
7361 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba, ma, this);
7362
7363
7364 assertEquals(biasX, calibrator.getBiasX(), 0.0);
7365 assertEquals(biasY, calibrator.getBiasY(), 0.0);
7366 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7367 final var bx1 = calibrator.getBiasXAsAcceleration();
7368 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7369 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7370 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7371 calibrator.getBiasXAsAcceleration(bx2);
7372 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7373 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7374 final var by1 = calibrator.getBiasYAsAcceleration();
7375 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7376 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7377 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7378 calibrator.getBiasYAsAcceleration(by2);
7379 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7380 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7381 final var bz1 = calibrator.getBiasZAsAcceleration();
7382 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7383 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7384 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7385 calibrator.getBiasZAsAcceleration(bz2);
7386 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7387 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7388 final var biasTriad1 = calibrator.getBiasAsTriad();
7389 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7390 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7391 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7392 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7393 final var biasTriad2 = new AccelerationTriad();
7394 calibrator.getBiasAsTriad(biasTriad2);
7395 assertEquals(biasTriad1, biasTriad2);
7396 assertEquals(sx, calibrator.getInitialSx(), 0.0);
7397 assertEquals(sy, calibrator.getInitialSy(), 0.0);
7398 assertEquals(sz, calibrator.getInitialSz(), 0.0);
7399 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
7400 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
7401 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
7402 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
7403 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
7404 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
7405 final var bias1 = calibrator.getBias();
7406 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7407 final var bias2 = new double[3];
7408 calibrator.getBias(bias2);
7409 assertArrayEquals(bias1, bias2, 0.0);
7410 final var b1 = calibrator.getBiasAsMatrix();
7411 assertEquals(b1, ba);
7412 final var b2 = new Matrix(3, 1);
7413 calibrator.getBiasAsMatrix(b2);
7414 assertEquals(b1, b2);
7415 final var ma1 = new Matrix(3, 3);
7416 ma1.setSubmatrix(0, 0, 2, 2,
7417 new double[]{sx, myx, mzx,
7418 mxy, sy, mzy,
7419 mxz, myz, sz});
7420 assertEquals(ma1, calibrator.getInitialMa());
7421 final var ma2 = new Matrix(3, 3);
7422 calibrator.getInitialMa(ma2);
7423 assertEquals(ma1, ma2);
7424 assertNull(calibrator.getMeasurements());
7425 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7426 calibrator.getMeasurementType());
7427 assertFalse(calibrator.isOrderedMeasurementsRequired());
7428 assertFalse(calibrator.isQualityScoresRequired());
7429 assertFalse(calibrator.isCommonAxisUsed());
7430 assertSame(this, calibrator.getListener());
7431 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
7432 assertFalse(calibrator.isReady());
7433 assertFalse(calibrator.isRunning());
7434 assertNull(calibrator.getEstimatedMa());
7435 assertNull(calibrator.getEstimatedSx());
7436 assertNull(calibrator.getEstimatedSy());
7437 assertNull(calibrator.getEstimatedSz());
7438 assertNull(calibrator.getEstimatedMxy());
7439 assertNull(calibrator.getEstimatedMxz());
7440 assertNull(calibrator.getEstimatedMyx());
7441 assertNull(calibrator.getEstimatedMyz());
7442 assertNull(calibrator.getEstimatedMzx());
7443 assertNull(calibrator.getEstimatedMzy());
7444 assertNull(calibrator.getEstimatedCovariance());
7445 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7446 assertNull(calibrator.getGroundTruthGravityNorm());
7447 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7448 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7449 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7450
7451
7452 final var m1 = new Matrix(1, 1);
7453 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7454 m1, ma, this));
7455 final var m2 = new Matrix(1, 3);
7456 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7457 m2, ma, this));
7458 final var m3 = new Matrix(1, 3);
7459 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(ba,
7460 m3, this));
7461 final var m4 = new Matrix(3, 1);
7462 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(ba,
7463 m4, this));
7464 }
7465
7466 @Test
7467 void testConstructor73() throws WrongSizeException {
7468 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
7469
7470 final var ba = generateBa();
7471 final var biasX = ba.getElementAtIndex(0);
7472 final var biasY = ba.getElementAtIndex(1);
7473 final var biasZ = ba.getElementAtIndex(2);
7474
7475 final var ma = generateMaCommonAxis();
7476 final var sx = ma.getElementAt(0, 0);
7477 final var sy = ma.getElementAt(1, 1);
7478 final var sz = ma.getElementAt(2, 2);
7479 final var mxy = ma.getElementAt(0, 1);
7480 final var mxz = ma.getElementAt(0, 2);
7481 final var myx = ma.getElementAt(1, 0);
7482 final var myz = ma.getElementAt(1, 2);
7483 final var mzx = ma.getElementAt(2, 0);
7484 final var mzy = ma.getElementAt(2, 1);
7485
7486 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, ba, ma);
7487
7488
7489 assertEquals(biasX, calibrator.getBiasX(), 0.0);
7490 assertEquals(biasY, calibrator.getBiasY(), 0.0);
7491 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7492 final var bx1 = calibrator.getBiasXAsAcceleration();
7493 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7494 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7495 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7496 calibrator.getBiasXAsAcceleration(bx2);
7497 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7498 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7499 final var by1 = calibrator.getBiasYAsAcceleration();
7500 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7501 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7502 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7503 calibrator.getBiasYAsAcceleration(by2);
7504 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7505 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7506 final var bz1 = calibrator.getBiasZAsAcceleration();
7507 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7508 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7509 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7510 calibrator.getBiasZAsAcceleration(bz2);
7511 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7512 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7513 final var biasTriad1 = calibrator.getBiasAsTriad();
7514 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7515 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7516 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7517 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7518 final var biasTriad2 = new AccelerationTriad();
7519 calibrator.getBiasAsTriad(biasTriad2);
7520 assertEquals(biasTriad1, biasTriad2);
7521 assertEquals(sx, calibrator.getInitialSx(), 0.0);
7522 assertEquals(sy, calibrator.getInitialSy(), 0.0);
7523 assertEquals(sz, calibrator.getInitialSz(), 0.0);
7524 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
7525 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
7526 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
7527 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
7528 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
7529 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
7530 final var bias1 = calibrator.getBias();
7531 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7532 final var bias2 = new double[3];
7533 calibrator.getBias(bias2);
7534 assertArrayEquals(bias1, bias2, 0.0);
7535 final var b1 = calibrator.getBiasAsMatrix();
7536 assertEquals(b1, ba);
7537 final var b2 = new Matrix(3, 1);
7538 calibrator.getBiasAsMatrix(b2);
7539 assertEquals(b1, b2);
7540 final var ma1 = new Matrix(3, 3);
7541 ma1.setSubmatrix(0, 0, 2, 2,
7542 new double[]{sx, myx, mzx,
7543 mxy, sy, mzy,
7544 mxz, myz, sz});
7545 assertEquals(ma1, calibrator.getInitialMa());
7546 final var ma2 = new Matrix(3, 3);
7547 calibrator.getInitialMa(ma2);
7548 assertEquals(ma1, ma2);
7549 assertSame(measurements, calibrator.getMeasurements());
7550 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7551 calibrator.getMeasurementType());
7552 assertFalse(calibrator.isOrderedMeasurementsRequired());
7553 assertFalse(calibrator.isQualityScoresRequired());
7554 assertFalse(calibrator.isCommonAxisUsed());
7555 assertNull(calibrator.getListener());
7556 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
7557 assertFalse(calibrator.isReady());
7558 assertFalse(calibrator.isRunning());
7559 assertNull(calibrator.getEstimatedMa());
7560 assertNull(calibrator.getEstimatedSx());
7561 assertNull(calibrator.getEstimatedSy());
7562 assertNull(calibrator.getEstimatedSz());
7563 assertNull(calibrator.getEstimatedMxy());
7564 assertNull(calibrator.getEstimatedMxz());
7565 assertNull(calibrator.getEstimatedMyx());
7566 assertNull(calibrator.getEstimatedMyz());
7567 assertNull(calibrator.getEstimatedMzx());
7568 assertNull(calibrator.getEstimatedMzy());
7569 assertNull(calibrator.getEstimatedCovariance());
7570 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7571 assertNull(calibrator.getGroundTruthGravityNorm());
7572 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7573 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7574 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7575
7576
7577 final var m1 = new Matrix(1, 1);
7578 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7579 measurements, m1, ma));
7580 final var m2 = new Matrix(1, 3);
7581 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7582 measurements, m2, ma));
7583 final var m3 = new Matrix(1, 3);
7584 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7585 measurements, ba, m3));
7586 final var m4 = new Matrix(3, 1);
7587 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7588 measurements, ba, m4));
7589 }
7590
7591 @Test
7592 void testConstructor74() throws WrongSizeException {
7593 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
7594
7595 final var ba = generateBa();
7596 final var biasX = ba.getElementAtIndex(0);
7597 final var biasY = ba.getElementAtIndex(1);
7598 final var biasZ = ba.getElementAtIndex(2);
7599
7600 final var ma = generateMaCommonAxis();
7601 final var sx = ma.getElementAt(0, 0);
7602 final var sy = ma.getElementAt(1, 1);
7603 final var sz = ma.getElementAt(2, 2);
7604 final var mxy = ma.getElementAt(0, 1);
7605 final var mxz = ma.getElementAt(0, 2);
7606 final var myx = ma.getElementAt(1, 0);
7607 final var myz = ma.getElementAt(1, 2);
7608 final var mzx = ma.getElementAt(2, 0);
7609 final var mzy = ma.getElementAt(2, 1);
7610
7611 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, ba, ma, this);
7612
7613
7614 assertEquals(calibrator.getBiasX(), biasX, 0.0);
7615 assertEquals(calibrator.getBiasY(), biasY, 0.0);
7616 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7617 final var bx1 = calibrator.getBiasXAsAcceleration();
7618 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7619 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7620 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7621 calibrator.getBiasXAsAcceleration(bx2);
7622 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7623 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7624 final var by1 = calibrator.getBiasYAsAcceleration();
7625 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7626 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7627 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7628 calibrator.getBiasYAsAcceleration(by2);
7629 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7630 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7631 final var bz1 = calibrator.getBiasZAsAcceleration();
7632 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7633 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7634 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7635 calibrator.getBiasZAsAcceleration(bz2);
7636 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7637 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7638 final var biasTriad1 = calibrator.getBiasAsTriad();
7639 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7640 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7641 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7642 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7643 final var biasTriad2 = new AccelerationTriad();
7644 calibrator.getBiasAsTriad(biasTriad2);
7645 assertEquals(biasTriad1, biasTriad2);
7646 assertEquals(sx, calibrator.getInitialSx(), 0.0);
7647 assertEquals(sy, calibrator.getInitialSy(), 0.0);
7648 assertEquals(sz, calibrator.getInitialSz(), 0.0);
7649 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
7650 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
7651 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
7652 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
7653 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
7654 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
7655 final var bias1 = calibrator.getBias();
7656 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7657 final var bias2 = new double[3];
7658 calibrator.getBias(bias2);
7659 assertArrayEquals(bias1, bias2, 0.0);
7660 final var b1 = calibrator.getBiasAsMatrix();
7661 assertEquals(b1, ba);
7662 final var b2 = new Matrix(3, 1);
7663 calibrator.getBiasAsMatrix(b2);
7664 assertEquals(b1, b2);
7665 final var ma1 = new Matrix(3, 3);
7666 ma1.setSubmatrix(0, 0, 2, 2,
7667 new double[]{sx, myx, mzx,
7668 mxy, sy, mzy,
7669 mxz, myz, sz});
7670 assertEquals(ma1, calibrator.getInitialMa());
7671 final var ma2 = new Matrix(3, 3);
7672 calibrator.getInitialMa(ma2);
7673 assertEquals(ma1, ma2);
7674 assertSame(measurements, calibrator.getMeasurements());
7675 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7676 calibrator.getMeasurementType());
7677 assertFalse(calibrator.isOrderedMeasurementsRequired());
7678 assertFalse(calibrator.isQualityScoresRequired());
7679 assertFalse(calibrator.isCommonAxisUsed());
7680 assertSame(this, calibrator.getListener());
7681 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
7682 assertFalse(calibrator.isReady());
7683 assertFalse(calibrator.isRunning());
7684 assertNull(calibrator.getEstimatedMa());
7685 assertNull(calibrator.getEstimatedSx());
7686 assertNull(calibrator.getEstimatedSy());
7687 assertNull(calibrator.getEstimatedSz());
7688 assertNull(calibrator.getEstimatedMxy());
7689 assertNull(calibrator.getEstimatedMxz());
7690 assertNull(calibrator.getEstimatedMyx());
7691 assertNull(calibrator.getEstimatedMyz());
7692 assertNull(calibrator.getEstimatedMzx());
7693 assertNull(calibrator.getEstimatedMzy());
7694 assertNull(calibrator.getEstimatedCovariance());
7695 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7696 assertNull(calibrator.getGroundTruthGravityNorm());
7697 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7698 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7699 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7700
7701
7702 final var m1 = new Matrix(1, 1);
7703 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7704 measurements, m1, ma, this));
7705 final var m2 = new Matrix(1, 3);
7706 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7707 measurements, m2, ma, this));
7708 final var m3 = new Matrix(1, 3);
7709 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7710 measurements, ba, m3, this));
7711 final var m4 = new Matrix(3, 1);
7712 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7713 measurements, ba, m4, this));
7714 }
7715
7716 @Test
7717 void testConstructor75() throws WrongSizeException {
7718 final var ba = generateBa();
7719 final var biasX = ba.getElementAtIndex(0);
7720 final var biasY = ba.getElementAtIndex(1);
7721 final var biasZ = ba.getElementAtIndex(2);
7722
7723 final var ma = generateMaCommonAxis();
7724 final var sx = ma.getElementAt(0, 0);
7725 final var sy = ma.getElementAt(1, 1);
7726 final var sz = ma.getElementAt(2, 2);
7727 final var mxy = ma.getElementAt(0, 1);
7728 final var mxz = ma.getElementAt(0, 2);
7729 final var myx = ma.getElementAt(1, 0);
7730 final var myz = ma.getElementAt(1, 2);
7731 final var mzx = ma.getElementAt(2, 0);
7732 final var mzy = ma.getElementAt(2, 1);
7733
7734 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, ba, ma);
7735
7736
7737 assertEquals(biasX, calibrator.getBiasX(), 0.0);
7738 assertEquals(biasY, calibrator.getBiasY(), 0.0);
7739 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7740 final var bx1 = calibrator.getBiasXAsAcceleration();
7741 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7742 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7743 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7744 calibrator.getBiasXAsAcceleration(bx2);
7745 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7746 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7747 final var by1 = calibrator.getBiasYAsAcceleration();
7748 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7749 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7750 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7751 calibrator.getBiasYAsAcceleration(by2);
7752 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7753 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7754 final var bz1 = calibrator.getBiasZAsAcceleration();
7755 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7756 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7757 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7758 calibrator.getBiasZAsAcceleration(bz2);
7759 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7760 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7761 final var biasTriad1 = calibrator.getBiasAsTriad();
7762 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7763 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7764 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7765 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7766 final var biasTriad2 = new AccelerationTriad();
7767 calibrator.getBiasAsTriad(biasTriad2);
7768 assertEquals(biasTriad1, biasTriad2);
7769 assertEquals(sx, calibrator.getInitialSx(), 0.0);
7770 assertEquals(sy, calibrator.getInitialSy(), 0.0);
7771 assertEquals(sz, calibrator.getInitialSz(), 0.0);
7772 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
7773 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
7774 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
7775 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
7776 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
7777 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
7778 final var bias1 = calibrator.getBias();
7779 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7780 final var bias2 = new double[3];
7781 calibrator.getBias(bias2);
7782 assertArrayEquals(bias1, bias2, 0.0);
7783 final var b1 = calibrator.getBiasAsMatrix();
7784 assertEquals(b1, ba);
7785 final var b2 = new Matrix(3, 1);
7786 calibrator.getBiasAsMatrix(b2);
7787 assertEquals(b1, b2);
7788 final var ma1 = new Matrix(3, 3);
7789 ma1.setSubmatrix(0, 0, 2, 2,
7790 new double[]{sx, myx, mzx,
7791 mxy, sy, mzy,
7792 mxz, myz, sz});
7793 assertEquals(calibrator.getInitialMa(), ma1);
7794 final var ma2 = new Matrix(3, 3);
7795 calibrator.getInitialMa(ma2);
7796 assertEquals(ma1, ma2);
7797 assertNull(calibrator.getMeasurements());
7798 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7799 calibrator.getMeasurementType());
7800 assertFalse(calibrator.isOrderedMeasurementsRequired());
7801 assertFalse(calibrator.isQualityScoresRequired());
7802 assertTrue(calibrator.isCommonAxisUsed());
7803 assertNull(calibrator.getListener());
7804 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
7805 assertFalse(calibrator.isReady());
7806 assertFalse(calibrator.isRunning());
7807 assertNull(calibrator.getEstimatedMa());
7808 assertNull(calibrator.getEstimatedSx());
7809 assertNull(calibrator.getEstimatedSy());
7810 assertNull(calibrator.getEstimatedSz());
7811 assertNull(calibrator.getEstimatedMxy());
7812 assertNull(calibrator.getEstimatedMxz());
7813 assertNull(calibrator.getEstimatedMyx());
7814 assertNull(calibrator.getEstimatedMyz());
7815 assertNull(calibrator.getEstimatedMzx());
7816 assertNull(calibrator.getEstimatedMzy());
7817 assertNull(calibrator.getEstimatedCovariance());
7818 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7819 assertNull(calibrator.getGroundTruthGravityNorm());
7820 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7821 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7822 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7823
7824
7825 final var m1 = new Matrix(1, 1);
7826 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7827 true, m1, ma));
7828 final var m2 = new Matrix(1, 3);
7829 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7830 true, m2, ma));
7831 final var m3 = new Matrix(1, 3);
7832 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7833 true, ba, m3));
7834 final var m4 = new Matrix(3, 1);
7835 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7836 true, ba, m4));
7837 }
7838
7839 @Test
7840 void testConstructor76() throws WrongSizeException {
7841 final var ba = generateBa();
7842 final var biasX = ba.getElementAtIndex(0);
7843 final var biasY = ba.getElementAtIndex(1);
7844 final var biasZ = ba.getElementAtIndex(2);
7845
7846 final var ma = generateMaCommonAxis();
7847 final var sx = ma.getElementAt(0, 0);
7848 final var sy = ma.getElementAt(1, 1);
7849 final var sz = ma.getElementAt(2, 2);
7850 final var mxy = ma.getElementAt(0, 1);
7851 final var mxz = ma.getElementAt(0, 2);
7852 final var myx = ma.getElementAt(1, 0);
7853 final var myz = ma.getElementAt(1, 2);
7854 final var mzx = ma.getElementAt(2, 0);
7855 final var mzy = ma.getElementAt(2, 1);
7856
7857 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, ba, ma,
7858 this);
7859
7860
7861 assertEquals(biasX, calibrator.getBiasX(), 0.0);
7862 assertEquals(biasY, calibrator.getBiasY(), 0.0);
7863 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7864 final var bx1 = calibrator.getBiasXAsAcceleration();
7865 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7866 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7867 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7868 calibrator.getBiasXAsAcceleration(bx2);
7869 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7870 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7871 final var by1 = calibrator.getBiasYAsAcceleration();
7872 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7873 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7874 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7875 calibrator.getBiasYAsAcceleration(by2);
7876 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7877 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7878 final var bz1 = calibrator.getBiasZAsAcceleration();
7879 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7880 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7881 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7882 calibrator.getBiasZAsAcceleration(bz2);
7883 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7884 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7885 final var biasTriad1 = calibrator.getBiasAsTriad();
7886 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7887 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7888 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7889 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7890 final var biasTriad2 = new AccelerationTriad();
7891 calibrator.getBiasAsTriad(biasTriad2);
7892 assertEquals(biasTriad1, biasTriad2);
7893 assertEquals(sx, calibrator.getInitialSx(), 0.0);
7894 assertEquals(sy, calibrator.getInitialSy(), 0.0);
7895 assertEquals(sz, calibrator.getInitialSz(), 0.0);
7896 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
7897 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
7898 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
7899 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
7900 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
7901 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
7902 final var bias1 = calibrator.getBias();
7903 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7904 final var bias2 = new double[3];
7905 calibrator.getBias(bias2);
7906 assertArrayEquals(bias1, bias2, 0.0);
7907 final var b1 = calibrator.getBiasAsMatrix();
7908 assertEquals(b1, ba);
7909 final var b2 = new Matrix(3, 1);
7910 calibrator.getBiasAsMatrix(b2);
7911 assertEquals(b1, b2);
7912 final var ma1 = new Matrix(3, 3);
7913 ma1.setSubmatrix(0, 0, 2, 2,
7914 new double[]{sx, myx, mzx,
7915 mxy, sy, mzy,
7916 mxz, myz, sz});
7917 assertEquals(ma1, calibrator.getInitialMa());
7918 final var ma2 = new Matrix(3, 3);
7919 calibrator.getInitialMa(ma2);
7920 assertEquals(ma1, ma2);
7921 assertNull(calibrator.getMeasurements());
7922 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7923 calibrator.getMeasurementType());
7924 assertFalse(calibrator.isOrderedMeasurementsRequired());
7925 assertFalse(calibrator.isQualityScoresRequired());
7926 assertTrue(calibrator.isCommonAxisUsed());
7927 assertSame(this, calibrator.getListener());
7928 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
7929 assertFalse(calibrator.isReady());
7930 assertFalse(calibrator.isRunning());
7931 assertNull(calibrator.getEstimatedMa());
7932 assertNull(calibrator.getEstimatedSx());
7933 assertNull(calibrator.getEstimatedSy());
7934 assertNull(calibrator.getEstimatedSz());
7935 assertNull(calibrator.getEstimatedMxy());
7936 assertNull(calibrator.getEstimatedMxz());
7937 assertNull(calibrator.getEstimatedMyx());
7938 assertNull(calibrator.getEstimatedMyz());
7939 assertNull(calibrator.getEstimatedMzx());
7940 assertNull(calibrator.getEstimatedMzy());
7941 assertNull(calibrator.getEstimatedCovariance());
7942 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7943 assertNull(calibrator.getGroundTruthGravityNorm());
7944 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7945 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7946 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7947
7948
7949 final var m1 = new Matrix(1, 1);
7950 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7951 true, m1, ma, this));
7952 final var m2 = new Matrix(1, 3);
7953 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7954 true, m2, ma, this));
7955 final var m3 = new Matrix(1, 3);
7956 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7957 true, ba, m3, this));
7958 final var m4 = new Matrix(3, 1);
7959 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7960 true, ba, m4, this));
7961 }
7962
7963 @Test
7964 void testConstructor77() throws WrongSizeException {
7965 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
7966
7967 final var ba = generateBa();
7968 final var biasX = ba.getElementAtIndex(0);
7969 final var biasY = ba.getElementAtIndex(1);
7970 final var biasZ = ba.getElementAtIndex(2);
7971
7972 final var ma = generateMaCommonAxis();
7973 final var sx = ma.getElementAt(0, 0);
7974 final var sy = ma.getElementAt(1, 1);
7975 final var sz = ma.getElementAt(2, 2);
7976 final var mxy = ma.getElementAt(0, 1);
7977 final var mxz = ma.getElementAt(0, 2);
7978 final var myx = ma.getElementAt(1, 0);
7979 final var myz = ma.getElementAt(1, 2);
7980 final var mzx = ma.getElementAt(2, 0);
7981 final var mzy = ma.getElementAt(2, 1);
7982
7983 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
7984 ba, ma);
7985
7986
7987 assertEquals(biasX, calibrator.getBiasX(), 0.0);
7988 assertEquals(biasY, calibrator.getBiasY(), 0.0);
7989 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7990 final var bx1 = calibrator.getBiasXAsAcceleration();
7991 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7992 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7993 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7994 calibrator.getBiasXAsAcceleration(bx2);
7995 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7996 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7997 final var by1 = calibrator.getBiasYAsAcceleration();
7998 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7999 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8000 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8001 calibrator.getBiasYAsAcceleration(by2);
8002 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
8003 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8004 final var bz1 = calibrator.getBiasZAsAcceleration();
8005 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
8006 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8007 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8008 calibrator.getBiasZAsAcceleration(bz2);
8009 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
8010 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8011 final var biasTriad1 = calibrator.getBiasAsTriad();
8012 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
8013 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
8014 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
8015 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8016 final var biasTriad2 = new AccelerationTriad();
8017 calibrator.getBiasAsTriad(biasTriad2);
8018 assertEquals(biasTriad1, biasTriad2);
8019 assertEquals(sx, calibrator.getInitialSx(), 0.0);
8020 assertEquals(sy, calibrator.getInitialSy(), 0.0);
8021 assertEquals(sz, calibrator.getInitialSz(), 0.0);
8022 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
8023 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
8024 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
8025 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
8026 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
8027 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
8028 final var bias1 = calibrator.getBias();
8029 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8030 final var bias2 = new double[3];
8031 calibrator.getBias(bias2);
8032 assertArrayEquals(bias1, bias2, 0.0);
8033 final var b1 = calibrator.getBiasAsMatrix();
8034 assertEquals(b1, ba);
8035 final var b2 = new Matrix(3, 1);
8036 calibrator.getBiasAsMatrix(b2);
8037 assertEquals(b1, b2);
8038 final var ma1 = new Matrix(3, 3);
8039 ma1.setSubmatrix(0, 0, 2, 2,
8040 new double[]{sx, myx, mzx,
8041 mxy, sy, mzy,
8042 mxz, myz, sz});
8043 assertEquals(ma1, calibrator.getInitialMa());
8044 final var ma2 = new Matrix(3, 3);
8045 calibrator.getInitialMa(ma2);
8046 assertEquals(ma1, ma2);
8047 assertSame(measurements, calibrator.getMeasurements());
8048 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8049 calibrator.getMeasurementType());
8050 assertFalse(calibrator.isOrderedMeasurementsRequired());
8051 assertFalse(calibrator.isQualityScoresRequired());
8052 assertTrue(calibrator.isCommonAxisUsed());
8053 assertNull(calibrator.getListener());
8054 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
8055 assertFalse(calibrator.isReady());
8056 assertFalse(calibrator.isRunning());
8057 assertNull(calibrator.getEstimatedMa());
8058 assertNull(calibrator.getEstimatedSx());
8059 assertNull(calibrator.getEstimatedSy());
8060 assertNull(calibrator.getEstimatedSz());
8061 assertNull(calibrator.getEstimatedMxy());
8062 assertNull(calibrator.getEstimatedMxz());
8063 assertNull(calibrator.getEstimatedMyx());
8064 assertNull(calibrator.getEstimatedMyz());
8065 assertNull(calibrator.getEstimatedMzx());
8066 assertNull(calibrator.getEstimatedMzy());
8067 assertNull(calibrator.getEstimatedCovariance());
8068 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8069 assertNull(calibrator.getGroundTruthGravityNorm());
8070 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8071 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8072 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8073
8074
8075 final var m1 = new Matrix(1, 1);
8076 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8077 measurements, true, m1, ma));
8078 final var m2 = new Matrix(1, 3);
8079 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8080 measurements, true, m2, ma));
8081 final var m3 = new Matrix(1, 3);
8082 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8083 measurements, true, ba, m3));
8084 final var m4 = new Matrix(3, 1);
8085 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8086 measurements, true, ba, m4));
8087 }
8088
8089 @Test
8090 void testConstructor78() throws WrongSizeException {
8091 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
8092
8093 final var ba = generateBa();
8094 final var biasX = ba.getElementAtIndex(0);
8095 final var biasY = ba.getElementAtIndex(1);
8096 final var biasZ = ba.getElementAtIndex(2);
8097
8098 final var ma = generateMaCommonAxis();
8099 final var sx = ma.getElementAt(0, 0);
8100 final var sy = ma.getElementAt(1, 1);
8101 final var sz = ma.getElementAt(2, 2);
8102 final var mxy = ma.getElementAt(0, 1);
8103 final var mxz = ma.getElementAt(0, 2);
8104 final var myx = ma.getElementAt(1, 0);
8105 final var myz = ma.getElementAt(1, 2);
8106 final var mzx = ma.getElementAt(2, 0);
8107 final var mzy = ma.getElementAt(2, 1);
8108
8109 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
8110 ba, ma, this);
8111
8112
8113 assertEquals(biasX, calibrator.getBiasX(), 0.0);
8114 assertEquals(biasY, calibrator.getBiasY(), 0.0);
8115 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
8116 final var bx1 = calibrator.getBiasXAsAcceleration();
8117 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
8118 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8119 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8120 calibrator.getBiasXAsAcceleration(bx2);
8121 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
8122 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8123 final var by1 = calibrator.getBiasYAsAcceleration();
8124 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
8125 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8126 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8127 calibrator.getBiasYAsAcceleration(by2);
8128 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
8129 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8130 final var bz1 = calibrator.getBiasZAsAcceleration();
8131 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
8132 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8133 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8134 calibrator.getBiasZAsAcceleration(bz2);
8135 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
8136 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8137 final var biasTriad1 = calibrator.getBiasAsTriad();
8138 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
8139 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
8140 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
8141 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8142 final var biasTriad2 = new AccelerationTriad();
8143 calibrator.getBiasAsTriad(biasTriad2);
8144 assertEquals(biasTriad1, biasTriad2);
8145 assertEquals(sx, calibrator.getInitialSx(), 0.0);
8146 assertEquals(sy, calibrator.getInitialSy(), 0.0);
8147 assertEquals(sz, calibrator.getInitialSz(), 0.0);
8148 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
8149 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
8150 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
8151 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
8152 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
8153 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
8154 final var bias1 = calibrator.getBias();
8155 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8156 final var bias2 = new double[3];
8157 calibrator.getBias(bias2);
8158 assertArrayEquals(bias1, bias2, 0.0);
8159 final var b1 = calibrator.getBiasAsMatrix();
8160 assertEquals(b1, ba);
8161 final var b2 = new Matrix(3, 1);
8162 calibrator.getBiasAsMatrix(b2);
8163 assertEquals(b1, b2);
8164 final var ma1 = new Matrix(3, 3);
8165 ma1.setSubmatrix(0, 0, 2, 2,
8166 new double[]{sx, myx, mzx,
8167 mxy, sy, mzy,
8168 mxz, myz, sz});
8169 assertEquals(ma1, calibrator.getInitialMa());
8170 final var ma2 = new Matrix(3, 3);
8171 calibrator.getInitialMa(ma2);
8172 assertEquals(ma1, ma2);
8173 assertSame(measurements, calibrator.getMeasurements());
8174 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8175 calibrator.getMeasurementType());
8176 assertFalse(calibrator.isOrderedMeasurementsRequired());
8177 assertFalse(calibrator.isQualityScoresRequired());
8178 assertTrue(calibrator.isCommonAxisUsed());
8179 assertSame(this, calibrator.getListener());
8180 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
8181 assertFalse(calibrator.isReady());
8182 assertFalse(calibrator.isRunning());
8183 assertNull(calibrator.getEstimatedMa());
8184 assertNull(calibrator.getEstimatedSx());
8185 assertNull(calibrator.getEstimatedSy());
8186 assertNull(calibrator.getEstimatedSz());
8187 assertNull(calibrator.getEstimatedMxy());
8188 assertNull(calibrator.getEstimatedMxz());
8189 assertNull(calibrator.getEstimatedMyx());
8190 assertNull(calibrator.getEstimatedMyz());
8191 assertNull(calibrator.getEstimatedMzx());
8192 assertNull(calibrator.getEstimatedMzy());
8193 assertNull(calibrator.getEstimatedCovariance());
8194 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8195 assertNull(calibrator.getGroundTruthGravityNorm());
8196 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8197 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8198 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8199
8200
8201 final var m1 = new Matrix(1, 1);
8202 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8203 measurements, true, m1, ma, this));
8204 final var m2 = new Matrix(1, 3);
8205 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8206 measurements, true, m2, ma, this));
8207 final var m3 = new Matrix(1, 3);
8208 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8209 measurements, true, ba, m3, this));
8210 final var m4 = new Matrix(3, 1);
8211 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8212 measurements, true, ba, m4, this));
8213 }
8214
8215 @Test
8216 void testConstructor79() throws WrongSizeException {
8217 final var randomizer = new UniformRandomizer();
8218 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8219 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8220 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8221 final var nedPosition = new NEDPosition(latitude, longitude, height);
8222 final var nedVelocity = new NEDVelocity();
8223 final var ecefPosition = new ECEFPosition();
8224 final var ecefVelocity = new ECEFVelocity();
8225 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
8226 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8227 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8228 final var gravityNorm = gravity.getNorm();
8229
8230 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm);
8231
8232
8233 assertEquals(0.0, calibrator.getBiasX(), 0.0);
8234 assertEquals(0.0, calibrator.getBiasY(), 0.0);
8235 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
8236 final var bx1 = calibrator.getBiasXAsAcceleration();
8237 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
8238 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8239 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8240 calibrator.getBiasXAsAcceleration(bx2);
8241 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
8242 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8243 final var by1 = calibrator.getBiasYAsAcceleration();
8244 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
8245 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8246 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8247 calibrator.getBiasYAsAcceleration(by2);
8248 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
8249 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8250 final var bz1 = calibrator.getBiasZAsAcceleration();
8251 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
8252 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8253 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8254 calibrator.getBiasZAsAcceleration(bz2);
8255 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
8256 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8257 final var biasTriad1 = calibrator.getBiasAsTriad();
8258 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
8259 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
8260 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
8261 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8262 final var biasTriad2 = new AccelerationTriad();
8263 calibrator.getBiasAsTriad(biasTriad2);
8264 assertEquals(biasTriad1, biasTriad2);
8265 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
8266 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
8267 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
8268 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
8269 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
8270 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
8271 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
8272 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
8273 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
8274 final var bias1 = calibrator.getBias();
8275 assertArrayEquals(new double[3], bias1, 0.0);
8276 final var bias2 = new double[3];
8277 calibrator.getBias(bias2);
8278 assertArrayEquals(bias1, bias2, 0.0);
8279 final var b1 = calibrator.getBiasAsMatrix();
8280 assertEquals(new Matrix(3, 1), b1);
8281 final var b2 = new Matrix(3, 1);
8282 calibrator.getBiasAsMatrix(b2);
8283 assertEquals(b1, b2);
8284 final var ma1 = calibrator.getInitialMa();
8285 assertEquals(new Matrix(3, 3), ma1);
8286 final var ma2 = new Matrix(3, 3);
8287 calibrator.getInitialMa(ma2);
8288 assertEquals(ma1, ma2);
8289 assertNull(calibrator.getMeasurements());
8290 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8291 calibrator.getMeasurementType());
8292 assertFalse(calibrator.isOrderedMeasurementsRequired());
8293 assertFalse(calibrator.isQualityScoresRequired());
8294 assertFalse(calibrator.isCommonAxisUsed());
8295 assertNull(calibrator.getListener());
8296 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
8297 assertFalse(calibrator.isReady());
8298 assertFalse(calibrator.isRunning());
8299 assertNull(calibrator.getEstimatedMa());
8300 assertNull(calibrator.getEstimatedSx());
8301 assertNull(calibrator.getEstimatedSy());
8302 assertNull(calibrator.getEstimatedSz());
8303 assertNull(calibrator.getEstimatedMxy());
8304 assertNull(calibrator.getEstimatedMxz());
8305 assertNull(calibrator.getEstimatedMyx());
8306 assertNull(calibrator.getEstimatedMyz());
8307 assertNull(calibrator.getEstimatedMzx());
8308 assertNull(calibrator.getEstimatedMzy());
8309 assertNull(calibrator.getEstimatedCovariance());
8310 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8311 assertNotNull(calibrator.getGroundTruthGravityNorm());
8312 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8313 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8314 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
8315 ABSOLUTE_ERROR));
8316 final var g = new Acceleration(0.0, AccelerationUnit.G);
8317 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8318 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8319 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8320
8321
8322 assertThrows(IllegalArgumentException.class,
8323 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(-gravityNorm));
8324 }
8325
8326 @Test
8327 void testConstructor80() throws WrongSizeException {
8328 final var randomizer = new UniformRandomizer();
8329 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8330 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8331 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8332 final var nedPosition = new NEDPosition(latitude, longitude, height);
8333 final var nedVelocity = new NEDVelocity();
8334 final var ecefPosition = new ECEFPosition();
8335 final var ecefVelocity = new ECEFVelocity();
8336 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
8337 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8338 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8339 final var gravityNorm = gravity.getNorm();
8340
8341 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, this);
8342
8343
8344 assertEquals(0.0, calibrator.getBiasX(), 0.0);
8345 assertEquals(0.0, calibrator.getBiasY(), 0.0);
8346 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
8347 final var bx1 = calibrator.getBiasXAsAcceleration();
8348 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
8349 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8350 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8351 calibrator.getBiasXAsAcceleration(bx2);
8352 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
8353 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8354 final var by1 = calibrator.getBiasYAsAcceleration();
8355 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
8356 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8357 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8358 calibrator.getBiasYAsAcceleration(by2);
8359 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
8360 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8361 final var bz1 = calibrator.getBiasZAsAcceleration();
8362 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
8363 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8364 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8365 calibrator.getBiasZAsAcceleration(bz2);
8366 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
8367 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8368 final var biasTriad1 = calibrator.getBiasAsTriad();
8369 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
8370 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
8371 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
8372 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8373 final var biasTriad2 = new AccelerationTriad();
8374 calibrator.getBiasAsTriad(biasTriad2);
8375 assertEquals(biasTriad1, biasTriad2);
8376 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
8377 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
8378 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
8379 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
8380 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
8381 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
8382 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
8383 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
8384 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
8385 final var bias1 = calibrator.getBias();
8386 assertArrayEquals(new double[3], bias1, 0.0);
8387 final var bias2 = new double[3];
8388 calibrator.getBias(bias2);
8389 assertArrayEquals(bias1, bias2, 0.0);
8390 final var b1 = calibrator.getBiasAsMatrix();
8391 assertEquals(new Matrix(3, 1), b1);
8392 final var b2 = new Matrix(3, 1);
8393 calibrator.getBiasAsMatrix(b2);
8394 assertEquals(b1, b2);
8395 final var ma1 = calibrator.getInitialMa();
8396 assertEquals(new Matrix(3, 3), ma1);
8397 final var ma2 = new Matrix(3, 3);
8398 calibrator.getInitialMa(ma2);
8399 assertEquals(ma1, ma2);
8400 assertNull(calibrator.getMeasurements());
8401 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8402 calibrator.getMeasurementType());
8403 assertFalse(calibrator.isOrderedMeasurementsRequired());
8404 assertFalse(calibrator.isQualityScoresRequired());
8405 assertFalse(calibrator.isCommonAxisUsed());
8406 assertSame(this, calibrator.getListener());
8407 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
8408 assertFalse(calibrator.isReady());
8409 assertFalse(calibrator.isRunning());
8410 assertNull(calibrator.getEstimatedMa());
8411 assertNull(calibrator.getEstimatedSx());
8412 assertNull(calibrator.getEstimatedSy());
8413 assertNull(calibrator.getEstimatedSz());
8414 assertNull(calibrator.getEstimatedMxy());
8415 assertNull(calibrator.getEstimatedMxz());
8416 assertNull(calibrator.getEstimatedMyx());
8417 assertNull(calibrator.getEstimatedMyz());
8418 assertNull(calibrator.getEstimatedMzx());
8419 assertNull(calibrator.getEstimatedMzy());
8420 assertNull(calibrator.getEstimatedCovariance());
8421 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8422 assertNotNull(calibrator.getGroundTruthGravityNorm());
8423 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8424 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8425 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
8426 ABSOLUTE_ERROR));
8427 final var g = new Acceleration(0.0, AccelerationUnit.G);
8428 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8429 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8430 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8431
8432
8433 assertThrows(IllegalArgumentException.class,
8434 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(-gravityNorm, this));
8435 }
8436
8437 @Test
8438 void testConstructor81() throws WrongSizeException {
8439 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
8440
8441 final var randomizer = new UniformRandomizer();
8442 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8443 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8444 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8445 final var nedPosition = new NEDPosition(latitude, longitude, height);
8446 final var nedVelocity = new NEDVelocity();
8447 final var ecefPosition = new ECEFPosition();
8448 final var ecefVelocity = new ECEFVelocity();
8449 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
8450 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8451 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8452 final var gravityNorm = gravity.getNorm();
8453
8454 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements);
8455
8456
8457 assertEquals(0.0, calibrator.getBiasX(), 0.0);
8458 assertEquals(0.0, calibrator.getBiasY(), 0.0);
8459 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
8460 final var bx1 = calibrator.getBiasXAsAcceleration();
8461 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
8462 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8463 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8464 calibrator.getBiasXAsAcceleration(bx2);
8465 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
8466 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8467 final var by1 = calibrator.getBiasYAsAcceleration();
8468 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
8469 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8470 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8471 calibrator.getBiasYAsAcceleration(by2);
8472 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
8473 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8474 final var bz1 = calibrator.getBiasZAsAcceleration();
8475 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
8476 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8477 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8478 calibrator.getBiasZAsAcceleration(bz2);
8479 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
8480 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8481 final var biasTriad1 = calibrator.getBiasAsTriad();
8482 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
8483 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
8484 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
8485 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8486 final var biasTriad2 = new AccelerationTriad();
8487 calibrator.getBiasAsTriad(biasTriad2);
8488 assertEquals(biasTriad1, biasTriad2);
8489 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
8490 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
8491 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
8492 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
8493 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
8494 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
8495 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
8496 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
8497 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
8498 final var bias1 = calibrator.getBias();
8499 assertArrayEquals(new double[3], bias1, 0.0);
8500 final var bias2 = new double[3];
8501 calibrator.getBias(bias2);
8502 assertArrayEquals(bias2, bias1, 0.0);
8503 final var b1 = calibrator.getBiasAsMatrix();
8504 assertEquals(new Matrix(3, 1), b1);
8505 final var b2 = new Matrix(3, 1);
8506 calibrator.getBiasAsMatrix(b2);
8507 assertEquals(b1, b2);
8508 final var ma1 = calibrator.getInitialMa();
8509 assertEquals(new Matrix(3, 3), ma1);
8510 final var ma2 = new Matrix(3, 3);
8511 calibrator.getInitialMa(ma2);
8512 assertEquals(ma1, ma2);
8513 assertSame(measurements, calibrator.getMeasurements());
8514 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8515 calibrator.getMeasurementType());
8516 assertFalse(calibrator.isOrderedMeasurementsRequired());
8517 assertFalse(calibrator.isQualityScoresRequired());
8518 assertFalse(calibrator.isCommonAxisUsed());
8519 assertNull(calibrator.getListener());
8520 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
8521 assertFalse(calibrator.isReady());
8522 assertFalse(calibrator.isRunning());
8523 assertNull(calibrator.getEstimatedMa());
8524 assertNull(calibrator.getEstimatedSx());
8525 assertNull(calibrator.getEstimatedSy());
8526 assertNull(calibrator.getEstimatedSz());
8527 assertNull(calibrator.getEstimatedMxy());
8528 assertNull(calibrator.getEstimatedMxz());
8529 assertNull(calibrator.getEstimatedMyx());
8530 assertNull(calibrator.getEstimatedMyz());
8531 assertNull(calibrator.getEstimatedMzx());
8532 assertNull(calibrator.getEstimatedMzy());
8533 assertNull(calibrator.getEstimatedCovariance());
8534 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8535 assertNotNull(calibrator.getGroundTruthGravityNorm());
8536 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8537 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8538 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
8539 ABSOLUTE_ERROR));
8540 final var g = new Acceleration(0.0, AccelerationUnit.G);
8541 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8542 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8543 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8544
8545
8546 assertThrows(IllegalArgumentException.class,
8547 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(-gravityNorm, measurements));
8548 }
8549
8550 @Test
8551 void testConstructor82() throws WrongSizeException {
8552 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
8553
8554 final var randomizer = new UniformRandomizer();
8555 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8556 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8557 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8558 final var nedPosition = new NEDPosition(latitude, longitude, height);
8559 final var nedVelocity = new NEDVelocity();
8560 final var ecefPosition = new ECEFPosition();
8561 final var ecefVelocity = new ECEFVelocity();
8562 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
8563 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8564 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8565 final var gravityNorm = gravity.getNorm();
8566
8567 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
8568 this);
8569
8570
8571 assertEquals(0.0, calibrator.getBiasX(), 0.0);
8572 assertEquals(0.0, calibrator.getBiasY(), 0.0);
8573 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
8574 final var bx1 = calibrator.getBiasXAsAcceleration();
8575 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
8576 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8577 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8578 calibrator.getBiasXAsAcceleration(bx2);
8579 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
8580 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8581 final var by1 = calibrator.getBiasYAsAcceleration();
8582 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
8583 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8584 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8585 calibrator.getBiasYAsAcceleration(by2);
8586 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
8587 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8588 final var bz1 = calibrator.getBiasZAsAcceleration();
8589 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
8590 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8591 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8592 calibrator.getBiasZAsAcceleration(bz2);
8593 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
8594 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8595 final var biasTriad1 = calibrator.getBiasAsTriad();
8596 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
8597 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
8598 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
8599 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8600 final var biasTriad2 = new AccelerationTriad();
8601 calibrator.getBiasAsTriad(biasTriad2);
8602 assertEquals(biasTriad1, biasTriad2);
8603 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
8604 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
8605 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
8606 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
8607 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
8608 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
8609 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
8610 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
8611 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
8612 final var bias1 = calibrator.getBias();
8613 assertArrayEquals(new double[3], bias1, 0.0);
8614 final var bias2 = new double[3];
8615 calibrator.getBias(bias2);
8616 assertArrayEquals(bias1, bias2, 0.0);
8617 final var b1 = calibrator.getBiasAsMatrix();
8618 assertEquals(new Matrix(3, 1), b1);
8619 final var b2 = new Matrix(3, 1);
8620 calibrator.getBiasAsMatrix(b2);
8621 assertEquals(b1, b2);
8622 final var ma1 = calibrator.getInitialMa();
8623 assertEquals(new Matrix(3, 3), ma1);
8624 final var ma2 = new Matrix(3, 3);
8625 calibrator.getInitialMa(ma2);
8626 assertEquals(ma1, ma2);
8627 assertSame(measurements, calibrator.getMeasurements());
8628 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8629 calibrator.getMeasurementType());
8630 assertFalse(calibrator.isOrderedMeasurementsRequired());
8631 assertFalse(calibrator.isQualityScoresRequired());
8632 assertFalse(calibrator.isCommonAxisUsed());
8633 assertSame(this, calibrator.getListener());
8634 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
8635 assertFalse(calibrator.isReady());
8636 assertFalse(calibrator.isRunning());
8637 assertNull(calibrator.getEstimatedMa());
8638 assertNull(calibrator.getEstimatedSx());
8639 assertNull(calibrator.getEstimatedSy());
8640 assertNull(calibrator.getEstimatedSz());
8641 assertNull(calibrator.getEstimatedMxy());
8642 assertNull(calibrator.getEstimatedMxz());
8643 assertNull(calibrator.getEstimatedMyx());
8644 assertNull(calibrator.getEstimatedMyz());
8645 assertNull(calibrator.getEstimatedMzx());
8646 assertNull(calibrator.getEstimatedMzy());
8647 assertNull(calibrator.getEstimatedCovariance());
8648 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8649 assertNotNull(calibrator.getGroundTruthGravityNorm());
8650 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8651 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8652 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
8653 ABSOLUTE_ERROR));
8654 final var g = new Acceleration(0.0, AccelerationUnit.G);
8655 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8656 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8657 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8658
8659
8660 assertThrows(IllegalArgumentException.class,
8661 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(-gravityNorm, measurements, this));
8662 }
8663
8664 @Test
8665 void testConstructor83() throws WrongSizeException {
8666 final var randomizer = new UniformRandomizer();
8667 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8668 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8669 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8670 final var nedPosition = new NEDPosition(latitude, longitude, height);
8671 final var nedVelocity = new NEDVelocity();
8672 final var ecefPosition = new ECEFPosition();
8673 final var ecefVelocity = new ECEFVelocity();
8674 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
8675 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8676 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8677 final var gravityNorm = gravity.getNorm();
8678
8679 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true);
8680
8681
8682 assertEquals(0.0, calibrator.getBiasX(), 0.0);
8683 assertEquals(0.0, calibrator.getBiasY(), 0.0);
8684 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
8685 final var bx1 = calibrator.getBiasXAsAcceleration();
8686 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
8687 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8688 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8689 calibrator.getBiasXAsAcceleration(bx2);
8690 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
8691 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8692 final var by1 = calibrator.getBiasYAsAcceleration();
8693 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
8694 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8695 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8696 calibrator.getBiasYAsAcceleration(by2);
8697 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
8698 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8699 final var bz1 = calibrator.getBiasZAsAcceleration();
8700 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
8701 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8702 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8703 calibrator.getBiasZAsAcceleration(bz2);
8704 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
8705 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8706 final var biasTriad1 = calibrator.getBiasAsTriad();
8707 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
8708 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
8709 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
8710 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8711 final var biasTriad2 = new AccelerationTriad();
8712 calibrator.getBiasAsTriad(biasTriad2);
8713 assertEquals(biasTriad1, biasTriad2);
8714 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
8715 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
8716 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
8717 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
8718 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
8719 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
8720 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
8721 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
8722 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
8723 final var bias1 = calibrator.getBias();
8724 assertArrayEquals(new double[3], bias1, 0.0);
8725 final var bias2 = new double[3];
8726 calibrator.getBias(bias2);
8727 assertArrayEquals(bias1, bias2, 0.0);
8728 final var b1 = calibrator.getBiasAsMatrix();
8729 assertEquals(new Matrix(3, 1), b1);
8730 final var b2 = new Matrix(3, 1);
8731 calibrator.getBiasAsMatrix(b2);
8732 assertEquals(b1, b2);
8733 final var ma1 = calibrator.getInitialMa();
8734 assertEquals(new Matrix(3, 3), ma1);
8735 final var ma2 = new Matrix(3, 3);
8736 calibrator.getInitialMa(ma2);
8737 assertEquals(ma1, ma2);
8738 assertNull(calibrator.getMeasurements());
8739 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8740 calibrator.getMeasurementType());
8741 assertFalse(calibrator.isOrderedMeasurementsRequired());
8742 assertFalse(calibrator.isQualityScoresRequired());
8743 assertTrue(calibrator.isCommonAxisUsed());
8744 assertNull(calibrator.getListener());
8745 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
8746 assertFalse(calibrator.isReady());
8747 assertFalse(calibrator.isRunning());
8748 assertNull(calibrator.getEstimatedMa());
8749 assertNull(calibrator.getEstimatedSx());
8750 assertNull(calibrator.getEstimatedSy());
8751 assertNull(calibrator.getEstimatedSz());
8752 assertNull(calibrator.getEstimatedMxy());
8753 assertNull(calibrator.getEstimatedMxz());
8754 assertNull(calibrator.getEstimatedMyx());
8755 assertNull(calibrator.getEstimatedMyz());
8756 assertNull(calibrator.getEstimatedMzx());
8757 assertNull(calibrator.getEstimatedMzy());
8758 assertNull(calibrator.getEstimatedCovariance());
8759 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8760 assertNotNull(calibrator.getGroundTruthGravityNorm());
8761 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8762 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8763 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
8764 ABSOLUTE_ERROR));
8765 final var g = new Acceleration(0.0, AccelerationUnit.G);
8766 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8767 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8768 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8769
8770
8771 assertThrows(IllegalArgumentException.class,
8772 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(-gravityNorm, true));
8773 }
8774
8775 @Test
8776 void testConstructor84() throws WrongSizeException {
8777 final var randomizer = new UniformRandomizer();
8778 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8779 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8780 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8781 final var nedPosition = new NEDPosition(latitude, longitude, height);
8782 final var nedVelocity = new NEDVelocity();
8783 final var ecefPosition = new ECEFPosition();
8784 final var ecefVelocity = new ECEFVelocity();
8785 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
8786 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8787 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8788 final var gravityNorm = gravity.getNorm();
8789
8790 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
8791 this);
8792
8793
8794 assertEquals(0.0, calibrator.getBiasX(), 0.0);
8795 assertEquals(0.0, calibrator.getBiasY(), 0.0);
8796 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
8797 final var bx1 = calibrator.getBiasXAsAcceleration();
8798 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
8799 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8800 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8801 calibrator.getBiasXAsAcceleration(bx2);
8802 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
8803 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8804 final var by1 = calibrator.getBiasYAsAcceleration();
8805 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
8806 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8807 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8808 calibrator.getBiasYAsAcceleration(by2);
8809 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
8810 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8811 final var bz1 = calibrator.getBiasZAsAcceleration();
8812 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
8813 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8814 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8815 calibrator.getBiasZAsAcceleration(bz2);
8816 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
8817 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8818 final var biasTriad1 = calibrator.getBiasAsTriad();
8819 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
8820 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
8821 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
8822 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8823 final var biasTriad2 = new AccelerationTriad();
8824 calibrator.getBiasAsTriad(biasTriad2);
8825 assertEquals(biasTriad1, biasTriad2);
8826 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
8827 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
8828 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
8829 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
8830 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
8831 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
8832 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
8833 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
8834 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
8835 final var bias1 = calibrator.getBias();
8836 assertArrayEquals(new double[3], bias1, 0.0);
8837 final var bias2 = new double[3];
8838 calibrator.getBias(bias2);
8839 assertArrayEquals(bias1, bias2, 0.0);
8840 final var b1 = calibrator.getBiasAsMatrix();
8841 assertEquals(new Matrix(3, 1), b1);
8842 final var b2 = new Matrix(3, 1);
8843 calibrator.getBiasAsMatrix(b2);
8844 assertEquals(b1, b2);
8845 final var ma1 = calibrator.getInitialMa();
8846 assertEquals(new Matrix(3, 3), ma1);
8847 final var ma2 = new Matrix(3, 3);
8848 calibrator.getInitialMa(ma2);
8849 assertEquals(ma1, ma2);
8850 assertNull(calibrator.getMeasurements());
8851 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8852 calibrator.getMeasurementType());
8853 assertFalse(calibrator.isOrderedMeasurementsRequired());
8854 assertFalse(calibrator.isQualityScoresRequired());
8855 assertTrue(calibrator.isCommonAxisUsed());
8856 assertSame(this, calibrator.getListener());
8857 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
8858 assertFalse(calibrator.isReady());
8859 assertFalse(calibrator.isRunning());
8860 assertNull(calibrator.getEstimatedMa());
8861 assertNull(calibrator.getEstimatedSx());
8862 assertNull(calibrator.getEstimatedSy());
8863 assertNull(calibrator.getEstimatedSz());
8864 assertNull(calibrator.getEstimatedMxy());
8865 assertNull(calibrator.getEstimatedMxz());
8866 assertNull(calibrator.getEstimatedMyx());
8867 assertNull(calibrator.getEstimatedMyz());
8868 assertNull(calibrator.getEstimatedMzx());
8869 assertNull(calibrator.getEstimatedMzy());
8870 assertNull(calibrator.getEstimatedCovariance());
8871 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8872 assertNotNull(calibrator.getGroundTruthGravityNorm());
8873 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8874 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8875 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
8876 ABSOLUTE_ERROR));
8877 final var g = new Acceleration(0.0, AccelerationUnit.G);
8878 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8879 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8880 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8881
8882
8883 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8884 -gravityNorm, true, this));
8885 }
8886
8887 @Test
8888 void testConstructor85() throws WrongSizeException {
8889 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
8890 final var randomizer = new UniformRandomizer();
8891 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8892 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8893 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8894 final var nedPosition = new NEDPosition(latitude, longitude, height);
8895 final var nedVelocity = new NEDVelocity();
8896 final var ecefPosition = new ECEFPosition();
8897 final var ecefVelocity = new ECEFVelocity();
8898 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
8899 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8900 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8901 final var gravityNorm = gravity.getNorm();
8902
8903 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
8904 true);
8905
8906
8907 assertEquals(0.0, calibrator.getBiasX(), 0.0);
8908 assertEquals(0.0, calibrator.getBiasY(), 0.0);
8909 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
8910 final var bx1 = calibrator.getBiasXAsAcceleration();
8911 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
8912 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8913 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8914 calibrator.getBiasXAsAcceleration(bx2);
8915 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
8916 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8917 final var by1 = calibrator.getBiasYAsAcceleration();
8918 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
8919 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8920 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8921 calibrator.getBiasYAsAcceleration(by2);
8922 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
8923 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8924 final var bz1 = calibrator.getBiasZAsAcceleration();
8925 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
8926 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8927 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8928 calibrator.getBiasZAsAcceleration(bz2);
8929 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
8930 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8931 final var biasTriad1 = calibrator.getBiasAsTriad();
8932 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
8933 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
8934 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
8935 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8936 final var biasTriad2 = new AccelerationTriad();
8937 calibrator.getBiasAsTriad(biasTriad2);
8938 assertEquals(biasTriad1, biasTriad2);
8939 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
8940 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
8941 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
8942 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
8943 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
8944 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
8945 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
8946 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
8947 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
8948 final var bias1 = calibrator.getBias();
8949 assertArrayEquals(new double[3], bias1, 0.0);
8950 final var bias2 = new double[3];
8951 calibrator.getBias(bias2);
8952 assertArrayEquals(bias1, bias2, 0.0);
8953 final var b1 = calibrator.getBiasAsMatrix();
8954 assertEquals(new Matrix(3, 1), b1);
8955 final var b2 = new Matrix(3, 1);
8956 calibrator.getBiasAsMatrix(b2);
8957 assertEquals(b1, b2);
8958 final var ma1 = calibrator.getInitialMa();
8959 assertEquals(new Matrix(3, 3), ma1);
8960 final var ma2 = new Matrix(3, 3);
8961 calibrator.getInitialMa(ma2);
8962 assertEquals(ma1, ma2);
8963 assertSame(measurements, calibrator.getMeasurements());
8964 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8965 calibrator.getMeasurementType());
8966 assertFalse(calibrator.isOrderedMeasurementsRequired());
8967 assertFalse(calibrator.isQualityScoresRequired());
8968 assertTrue(calibrator.isCommonAxisUsed());
8969 assertNull(calibrator.getListener());
8970 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
8971 assertFalse(calibrator.isReady());
8972 assertFalse(calibrator.isRunning());
8973 assertNull(calibrator.getEstimatedMa());
8974 assertNull(calibrator.getEstimatedSx());
8975 assertNull(calibrator.getEstimatedSy());
8976 assertNull(calibrator.getEstimatedSz());
8977 assertNull(calibrator.getEstimatedMxy());
8978 assertNull(calibrator.getEstimatedMxz());
8979 assertNull(calibrator.getEstimatedMyx());
8980 assertNull(calibrator.getEstimatedMyz());
8981 assertNull(calibrator.getEstimatedMzx());
8982 assertNull(calibrator.getEstimatedMzy());
8983 assertNull(calibrator.getEstimatedCovariance());
8984 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8985 assertNotNull(calibrator.getGroundTruthGravityNorm());
8986 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8987 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8988 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
8989 ABSOLUTE_ERROR));
8990 final var g = new Acceleration(0.0, AccelerationUnit.G);
8991 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8992 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8993 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8994
8995
8996 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8997 -gravityNorm, measurements, true));
8998 }
8999
9000 @Test
9001 void testConstructor86() throws WrongSizeException {
9002 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
9003 final var randomizer = new UniformRandomizer();
9004 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9005 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9006 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9007 final var nedPosition = new NEDPosition(latitude, longitude, height);
9008 final var nedVelocity = new NEDVelocity();
9009 final var ecefPosition = new ECEFPosition();
9010 final var ecefVelocity = new ECEFVelocity();
9011 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9012 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9013 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9014 final var gravityNorm = gravity.getNorm();
9015
9016 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
9017 true, this);
9018
9019
9020 assertEquals(0.0, calibrator.getBiasX(), 0.0);
9021 assertEquals(0.0, calibrator.getBiasY(), 0.0);
9022 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
9023 final var bx1 = calibrator.getBiasXAsAcceleration();
9024 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
9025 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9026 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9027 calibrator.getBiasXAsAcceleration(bx2);
9028 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
9029 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9030 final var by1 = calibrator.getBiasYAsAcceleration();
9031 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
9032 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9033 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9034 calibrator.getBiasYAsAcceleration(by2);
9035 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
9036 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9037 final var bz1 = calibrator.getBiasZAsAcceleration();
9038 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
9039 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9040 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9041 calibrator.getBiasZAsAcceleration(bz2);
9042 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
9043 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9044 final var biasTriad1 = calibrator.getBiasAsTriad();
9045 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
9046 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
9047 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
9048 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9049 final var biasTriad2 = new AccelerationTriad();
9050 calibrator.getBiasAsTriad(biasTriad2);
9051 assertEquals(biasTriad1, biasTriad2);
9052 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9053 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9054 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9055 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9056 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9057 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9058 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9059 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9060 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9061 final var bias1 = calibrator.getBias();
9062 assertArrayEquals(new double[3], bias1, 0.0);
9063 final var bias2 = new double[3];
9064 calibrator.getBias(bias2);
9065 assertArrayEquals(bias1, bias2, 0.0);
9066 final var b1 = calibrator.getBiasAsMatrix();
9067 assertEquals(new Matrix(3, 1), b1);
9068 final var b2 = new Matrix(3, 1);
9069 calibrator.getBiasAsMatrix(b2);
9070 assertEquals(b1, b2);
9071 final var ma1 = calibrator.getInitialMa();
9072 assertEquals(new Matrix(3, 3), ma1);
9073 final var ma2 = new Matrix(3, 3);
9074 calibrator.getInitialMa(ma2);
9075 assertEquals(ma1, ma2);
9076 assertSame(measurements, calibrator.getMeasurements());
9077 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9078 calibrator.getMeasurementType());
9079 assertFalse(calibrator.isOrderedMeasurementsRequired());
9080 assertFalse(calibrator.isQualityScoresRequired());
9081 assertTrue(calibrator.isCommonAxisUsed());
9082 assertSame(this, calibrator.getListener());
9083 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
9084 assertFalse(calibrator.isReady());
9085 assertFalse(calibrator.isRunning());
9086 assertNull(calibrator.getEstimatedMa());
9087 assertNull(calibrator.getEstimatedSx());
9088 assertNull(calibrator.getEstimatedSy());
9089 assertNull(calibrator.getEstimatedSz());
9090 assertNull(calibrator.getEstimatedMxy());
9091 assertNull(calibrator.getEstimatedMxz());
9092 assertNull(calibrator.getEstimatedMyx());
9093 assertNull(calibrator.getEstimatedMyz());
9094 assertNull(calibrator.getEstimatedMzx());
9095 assertNull(calibrator.getEstimatedMzy());
9096 assertNull(calibrator.getEstimatedCovariance());
9097 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9098 assertNotNull(calibrator.getGroundTruthGravityNorm());
9099 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9100 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9101 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9102 ABSOLUTE_ERROR));
9103 final var g = new Acceleration(0.0, AccelerationUnit.G);
9104 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9105 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9106 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9107
9108
9109 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
9110 -gravityNorm, measurements, true, this));
9111 }
9112
9113 @Test
9114 void testConstructor87() throws WrongSizeException {
9115 final var ba = generateBa();
9116 final var biasX = ba.getElementAtIndex(0);
9117 final var biasY = ba.getElementAtIndex(1);
9118 final var biasZ = ba.getElementAtIndex(2);
9119
9120 final var randomizer = new UniformRandomizer();
9121 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9122 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9123 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9124 final var nedPosition = new NEDPosition(latitude, longitude, height);
9125 final var nedVelocity = new NEDVelocity();
9126 final var ecefPosition = new ECEFPosition();
9127 final var ecefVelocity = new ECEFVelocity();
9128 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9129 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9130 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9131 final var gravityNorm = gravity.getNorm();
9132
9133 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ);
9134
9135
9136 assertEquals(biasX, calibrator.getBiasX(), 0.0);
9137 assertEquals(biasY, calibrator.getBiasY(), 0.0);
9138 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9139 final var bx1 = calibrator.getBiasXAsAcceleration();
9140 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9141 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9142 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9143 calibrator.getBiasXAsAcceleration(bx2);
9144 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9145 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9146 final var by1 = calibrator.getBiasYAsAcceleration();
9147 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9148 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9149 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9150 calibrator.getBiasYAsAcceleration(by2);
9151 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9152 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9153 final var bz1 = calibrator.getBiasZAsAcceleration();
9154 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9155 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9156 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9157 calibrator.getBiasZAsAcceleration(bz2);
9158 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9159 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9160 final var biasTriad1 = calibrator.getBiasAsTriad();
9161 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9162 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9163 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9164 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9165 final var biasTriad2 = new AccelerationTriad();
9166 calibrator.getBiasAsTriad(biasTriad2);
9167 assertEquals(biasTriad1, biasTriad2);
9168 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9169 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9170 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9171 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9172 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9173 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9174 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9175 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9176 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9177 final var bias1 = calibrator.getBias();
9178 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9179 final var bias2 = new double[3];
9180 calibrator.getBias(bias2);
9181 assertArrayEquals(bias1, bias2, 0.0);
9182 final var b1 = calibrator.getBiasAsMatrix();
9183 assertEquals(b1, ba);
9184 final var b2 = new Matrix(3, 1);
9185 calibrator.getBiasAsMatrix(b2);
9186 assertEquals(b1, b2);
9187 final var ma1 = calibrator.getInitialMa();
9188 assertEquals(new Matrix(3, 3), ma1);
9189 final var ma2 = new Matrix(3, 3);
9190 calibrator.getInitialMa(ma2);
9191 assertEquals(ma1, ma2);
9192 assertNull(calibrator.getMeasurements());
9193 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9194 calibrator.getMeasurementType());
9195 assertFalse(calibrator.isOrderedMeasurementsRequired());
9196 assertFalse(calibrator.isQualityScoresRequired());
9197 assertFalse(calibrator.isCommonAxisUsed());
9198 assertNull(calibrator.getListener());
9199 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
9200 assertFalse(calibrator.isReady());
9201 assertFalse(calibrator.isRunning());
9202 assertNull(calibrator.getEstimatedMa());
9203 assertNull(calibrator.getEstimatedSx());
9204 assertNull(calibrator.getEstimatedSy());
9205 assertNull(calibrator.getEstimatedSz());
9206 assertNull(calibrator.getEstimatedMxy());
9207 assertNull(calibrator.getEstimatedMxz());
9208 assertNull(calibrator.getEstimatedMyx());
9209 assertNull(calibrator.getEstimatedMyz());
9210 assertNull(calibrator.getEstimatedMzx());
9211 assertNull(calibrator.getEstimatedMzy());
9212 assertNull(calibrator.getEstimatedCovariance());
9213 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9214 assertNotNull(calibrator.getGroundTruthGravityNorm());
9215 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9216 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9217 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9218 ABSOLUTE_ERROR));
9219 final var g = new Acceleration(0.0, AccelerationUnit.G);
9220 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9221 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9222 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9223
9224
9225 assertThrows(IllegalArgumentException.class,
9226 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(-gravityNorm, biasX, biasY, biasZ));
9227 }
9228
9229 @Test
9230 void testConstructor88() throws WrongSizeException {
9231 final var ba = generateBa();
9232 final var biasX = ba.getElementAtIndex(0);
9233 final var biasY = ba.getElementAtIndex(1);
9234 final var biasZ = ba.getElementAtIndex(2);
9235
9236 final var randomizer = new UniformRandomizer();
9237 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9238 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9239 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9240 final var nedPosition = new NEDPosition(latitude, longitude, height);
9241 final var nedVelocity = new NEDVelocity();
9242 final var ecefPosition = new ECEFPosition();
9243 final var ecefVelocity = new ECEFVelocity();
9244 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9245 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9246 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9247 final var gravityNorm = gravity.getNorm();
9248
9249 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ,
9250 this);
9251
9252
9253 assertEquals(biasX, calibrator.getBiasX(), 0.0);
9254 assertEquals(biasY, calibrator.getBiasY(), 0.0);
9255 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9256 final var bx1 = calibrator.getBiasXAsAcceleration();
9257 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9258 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9259 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9260 calibrator.getBiasXAsAcceleration(bx2);
9261 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9262 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9263 final var by1 = calibrator.getBiasYAsAcceleration();
9264 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9265 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9266 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9267 calibrator.getBiasYAsAcceleration(by2);
9268 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9269 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9270 final var bz1 = calibrator.getBiasZAsAcceleration();
9271 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9272 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9273 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9274 calibrator.getBiasZAsAcceleration(bz2);
9275 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9276 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9277 final var biasTriad1 = calibrator.getBiasAsTriad();
9278 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9279 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9280 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9281 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9282 final var biasTriad2 = new AccelerationTriad();
9283 calibrator.getBiasAsTriad(biasTriad2);
9284 assertEquals(biasTriad1, biasTriad2);
9285 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9286 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9287 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9288 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9289 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9290 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9291 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9292 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9293 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9294 final var bias1 = calibrator.getBias();
9295 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9296 final var bias2 = new double[3];
9297 calibrator.getBias(bias2);
9298 assertArrayEquals(bias1, bias2, 0.0);
9299 final var b1 = calibrator.getBiasAsMatrix();
9300 assertEquals(b1, ba);
9301 final var b2 = new Matrix(3, 1);
9302 calibrator.getBiasAsMatrix(b2);
9303 assertEquals(b1, b2);
9304 final var ma1 = calibrator.getInitialMa();
9305 assertEquals(new Matrix(3, 3), ma1);
9306 final var ma2 = new Matrix(3, 3);
9307 calibrator.getInitialMa(ma2);
9308 assertEquals(ma1, ma2);
9309 assertNull(calibrator.getMeasurements());
9310 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9311 calibrator.getMeasurementType());
9312 assertFalse(calibrator.isOrderedMeasurementsRequired());
9313 assertFalse(calibrator.isQualityScoresRequired());
9314 assertFalse(calibrator.isCommonAxisUsed());
9315 assertSame(this, calibrator.getListener());
9316 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
9317 assertFalse(calibrator.isReady());
9318 assertFalse(calibrator.isRunning());
9319 assertNull(calibrator.getEstimatedMa());
9320 assertNull(calibrator.getEstimatedSx());
9321 assertNull(calibrator.getEstimatedSy());
9322 assertNull(calibrator.getEstimatedSz());
9323 assertNull(calibrator.getEstimatedMxy());
9324 assertNull(calibrator.getEstimatedMxz());
9325 assertNull(calibrator.getEstimatedMyx());
9326 assertNull(calibrator.getEstimatedMyz());
9327 assertNull(calibrator.getEstimatedMzx());
9328 assertNull(calibrator.getEstimatedMzy());
9329 assertNull(calibrator.getEstimatedCovariance());
9330 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9331 assertNotNull(calibrator.getGroundTruthGravityNorm());
9332 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9333 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9334 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9335 ABSOLUTE_ERROR));
9336 final var g = new Acceleration(0.0, AccelerationUnit.G);
9337 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9338 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9339 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9340
9341
9342 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
9343 -gravityNorm, biasX, biasY, biasZ, this));
9344 }
9345
9346 @Test
9347 void testConstructor89() throws WrongSizeException {
9348 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
9349
9350 final var ba = generateBa();
9351 final var biasX = ba.getElementAtIndex(0);
9352 final var biasY = ba.getElementAtIndex(1);
9353 final var biasZ = ba.getElementAtIndex(2);
9354
9355 final var randomizer = new UniformRandomizer();
9356 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9357 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9358 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9359 final var nedPosition = new NEDPosition(latitude, longitude, height);
9360 final var nedVelocity = new NEDVelocity();
9361 final var ecefPosition = new ECEFPosition();
9362 final var ecefVelocity = new ECEFVelocity();
9363 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9364 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9365 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9366 final var gravityNorm = gravity.getNorm();
9367
9368 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
9369 biasX, biasY, biasZ);
9370
9371
9372 assertEquals(biasX, calibrator.getBiasX(), 0.0);
9373 assertEquals(biasY, calibrator.getBiasY(), 0.0);
9374 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9375 final var bx1 = calibrator.getBiasXAsAcceleration();
9376 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9377 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9378 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9379 calibrator.getBiasXAsAcceleration(bx2);
9380 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9381 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9382 final var by1 = calibrator.getBiasYAsAcceleration();
9383 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9384 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9385 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9386 calibrator.getBiasYAsAcceleration(by2);
9387 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9388 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9389 final var bz1 = calibrator.getBiasZAsAcceleration();
9390 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9391 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9392 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9393 calibrator.getBiasZAsAcceleration(bz2);
9394 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9395 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9396 final var biasTriad1 = calibrator.getBiasAsTriad();
9397 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9398 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9399 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9400 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9401 final var biasTriad2 = new AccelerationTriad();
9402 calibrator.getBiasAsTriad(biasTriad2);
9403 assertEquals(biasTriad1, biasTriad2);
9404 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9405 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9406 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9407 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9408 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9409 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9410 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9411 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9412 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9413 final var bias1 = calibrator.getBias();
9414 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9415 final var bias2 = new double[3];
9416 calibrator.getBias(bias2);
9417 assertArrayEquals(bias1, bias2, 0.0);
9418 final var b1 = calibrator.getBiasAsMatrix();
9419 assertEquals(b1, ba);
9420 final var b2 = new Matrix(3, 1);
9421 calibrator.getBiasAsMatrix(b2);
9422 assertEquals(b1, b2);
9423 final var ma1 = calibrator.getInitialMa();
9424 assertEquals(new Matrix(3, 3), ma1);
9425 final var ma2 = new Matrix(3, 3);
9426 calibrator.getInitialMa(ma2);
9427 assertEquals(ma1, ma2);
9428 assertSame(measurements, calibrator.getMeasurements());
9429 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9430 calibrator.getMeasurementType());
9431 assertFalse(calibrator.isOrderedMeasurementsRequired());
9432 assertFalse(calibrator.isQualityScoresRequired());
9433 assertFalse(calibrator.isCommonAxisUsed());
9434 assertNull(calibrator.getListener());
9435 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
9436 assertFalse(calibrator.isReady());
9437 assertFalse(calibrator.isRunning());
9438 assertNull(calibrator.getEstimatedMa());
9439 assertNull(calibrator.getEstimatedSx());
9440 assertNull(calibrator.getEstimatedSy());
9441 assertNull(calibrator.getEstimatedSz());
9442 assertNull(calibrator.getEstimatedMxy());
9443 assertNull(calibrator.getEstimatedMxz());
9444 assertNull(calibrator.getEstimatedMyx());
9445 assertNull(calibrator.getEstimatedMyz());
9446 assertNull(calibrator.getEstimatedMzx());
9447 assertNull(calibrator.getEstimatedMzy());
9448 assertNull(calibrator.getEstimatedCovariance());
9449 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9450 assertNotNull(calibrator.getGroundTruthGravityNorm());
9451 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9452 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9453 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9454 ABSOLUTE_ERROR));
9455 final var g = new Acceleration(0.0, AccelerationUnit.G);
9456 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9457 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9458 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9459
9460
9461 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
9462 -gravityNorm, measurements, biasX, biasY, biasZ));
9463 }
9464
9465 @Test
9466 void testConstructor90() throws WrongSizeException {
9467 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
9468
9469 final var ba = generateBa();
9470 final var biasX = ba.getElementAtIndex(0);
9471 final var biasY = ba.getElementAtIndex(1);
9472 final var biasZ = ba.getElementAtIndex(2);
9473
9474 final var randomizer = new UniformRandomizer();
9475 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9476 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9477 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9478 final var nedPosition = new NEDPosition(latitude, longitude, height);
9479 final var nedVelocity = new NEDVelocity();
9480 final var ecefPosition = new ECEFPosition();
9481 final var ecefVelocity = new ECEFVelocity();
9482 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9483 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9484 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9485 final var gravityNorm = gravity.getNorm();
9486
9487 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
9488 biasX, biasY, biasZ, this);
9489
9490
9491 assertEquals(biasX, calibrator.getBiasX(), 0.0);
9492 assertEquals(biasY, calibrator.getBiasY(), 0.0);
9493 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9494 final var bx1 = calibrator.getBiasXAsAcceleration();
9495 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9496 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9497 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9498 calibrator.getBiasXAsAcceleration(bx2);
9499 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9500 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9501 final var by1 = calibrator.getBiasYAsAcceleration();
9502 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9503 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9504 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9505 calibrator.getBiasYAsAcceleration(by2);
9506 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9507 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9508 final var bz1 = calibrator.getBiasZAsAcceleration();
9509 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9510 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9511 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9512 calibrator.getBiasZAsAcceleration(bz2);
9513 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9514 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9515 final var biasTriad1 = calibrator.getBiasAsTriad();
9516 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9517 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9518 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9519 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9520 final var biasTriad2 = new AccelerationTriad();
9521 calibrator.getBiasAsTriad(biasTriad2);
9522 assertEquals(biasTriad1, biasTriad2);
9523 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9524 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9525 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9526 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9527 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9528 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9529 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9530 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9531 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9532 final var bias1 = calibrator.getBias();
9533 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9534 final var bias2 = new double[3];
9535 calibrator.getBias(bias2);
9536 assertArrayEquals(bias1, bias2, 0.0);
9537 final var b1 = calibrator.getBiasAsMatrix();
9538 assertEquals(b1, ba);
9539 final var b2 = new Matrix(3, 1);
9540 calibrator.getBiasAsMatrix(b2);
9541 assertEquals(b1, b2);
9542 final var ma1 = calibrator.getInitialMa();
9543 assertEquals(new Matrix(3, 3), ma1);
9544 final var ma2 = new Matrix(3, 3);
9545 calibrator.getInitialMa(ma2);
9546 assertEquals(ma1, ma2);
9547 assertSame(measurements, calibrator.getMeasurements());
9548 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9549 calibrator.getMeasurementType());
9550 assertFalse(calibrator.isOrderedMeasurementsRequired());
9551 assertFalse(calibrator.isQualityScoresRequired());
9552 assertFalse(calibrator.isCommonAxisUsed());
9553 assertSame(this, calibrator.getListener());
9554 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
9555 assertFalse(calibrator.isReady());
9556 assertFalse(calibrator.isRunning());
9557 assertNull(calibrator.getEstimatedMa());
9558 assertNull(calibrator.getEstimatedSx());
9559 assertNull(calibrator.getEstimatedSy());
9560 assertNull(calibrator.getEstimatedSz());
9561 assertNull(calibrator.getEstimatedMxy());
9562 assertNull(calibrator.getEstimatedMxz());
9563 assertNull(calibrator.getEstimatedMyx());
9564 assertNull(calibrator.getEstimatedMyz());
9565 assertNull(calibrator.getEstimatedMzx());
9566 assertNull(calibrator.getEstimatedMzy());
9567 assertNull(calibrator.getEstimatedCovariance());
9568 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9569 assertNotNull(calibrator.getGroundTruthGravityNorm());
9570 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9571 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9572 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9573 ABSOLUTE_ERROR));
9574 final var g = new Acceleration(0.0, AccelerationUnit.G);
9575 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9576 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9577 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9578
9579
9580 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
9581 -gravityNorm, measurements, biasX, biasY, biasZ, this));
9582 }
9583
9584 @Test
9585 void testConstructor91() throws WrongSizeException {
9586 final var ba = generateBa();
9587 final var biasX = ba.getElementAtIndex(0);
9588 final var biasY = ba.getElementAtIndex(1);
9589 final var biasZ = ba.getElementAtIndex(2);
9590
9591 final var randomizer = new UniformRandomizer();
9592 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9593 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9594 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9595 final var nedPosition = new NEDPosition(latitude, longitude, height);
9596 final var nedVelocity = new NEDVelocity();
9597 final var ecefPosition = new ECEFPosition();
9598 final var ecefVelocity = new ECEFVelocity();
9599 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9600 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9601 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9602 final var gravityNorm = gravity.getNorm();
9603
9604 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
9605 biasX, biasY, biasZ);
9606
9607
9608 assertEquals(biasX, calibrator.getBiasX(), 0.0);
9609 assertEquals(biasY, calibrator.getBiasY(), 0.0);
9610 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9611 final var bx1 = calibrator.getBiasXAsAcceleration();
9612 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9613 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9614 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9615 calibrator.getBiasXAsAcceleration(bx2);
9616 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9617 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9618 final var by1 = calibrator.getBiasYAsAcceleration();
9619 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9620 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9621 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9622 calibrator.getBiasYAsAcceleration(by2);
9623 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9624 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9625 final var bz1 = calibrator.getBiasZAsAcceleration();
9626 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9627 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9628 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9629 calibrator.getBiasZAsAcceleration(bz2);
9630 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9631 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9632 final var biasTriad1 = calibrator.getBiasAsTriad();
9633 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9634 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9635 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9636 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9637 final var biasTriad2 = new AccelerationTriad();
9638 calibrator.getBiasAsTriad(biasTriad2);
9639 assertEquals(biasTriad1, biasTriad2);
9640 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9641 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9642 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9643 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9644 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9645 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9646 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9647 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9648 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9649 final var bias1 = calibrator.getBias();
9650 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9651 final var bias2 = new double[3];
9652 calibrator.getBias(bias2);
9653 assertArrayEquals(bias1, bias2, 0.0);
9654 final var b1 = calibrator.getBiasAsMatrix();
9655 assertEquals(b1, ba);
9656 final var b2 = new Matrix(3, 1);
9657 calibrator.getBiasAsMatrix(b2);
9658 assertEquals(b1, b2);
9659 final var ma1 = calibrator.getInitialMa();
9660 assertEquals(new Matrix(3, 3), ma1);
9661 final var ma2 = new Matrix(3, 3);
9662 calibrator.getInitialMa(ma2);
9663 assertEquals(ma1, ma2);
9664 assertNull(calibrator.getMeasurements());
9665 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9666 calibrator.getMeasurementType());
9667 assertFalse(calibrator.isOrderedMeasurementsRequired());
9668 assertFalse(calibrator.isQualityScoresRequired());
9669 assertTrue(calibrator.isCommonAxisUsed());
9670 assertNull(calibrator.getListener());
9671 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
9672 assertFalse(calibrator.isReady());
9673 assertFalse(calibrator.isRunning());
9674 assertNull(calibrator.getEstimatedMa());
9675 assertNull(calibrator.getEstimatedSx());
9676 assertNull(calibrator.getEstimatedSy());
9677 assertNull(calibrator.getEstimatedSz());
9678 assertNull(calibrator.getEstimatedMxy());
9679 assertNull(calibrator.getEstimatedMxz());
9680 assertNull(calibrator.getEstimatedMyx());
9681 assertNull(calibrator.getEstimatedMyz());
9682 assertNull(calibrator.getEstimatedMzx());
9683 assertNull(calibrator.getEstimatedMzy());
9684 assertNull(calibrator.getEstimatedCovariance());
9685 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9686 assertNotNull(calibrator.getGroundTruthGravityNorm());
9687 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9688 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9689 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9690 ABSOLUTE_ERROR));
9691 final var g = new Acceleration(0.0, AccelerationUnit.G);
9692 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9693 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9694 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9695
9696
9697 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
9698 -gravityNorm, true, biasX, biasY, biasZ));
9699 }
9700
9701 @Test
9702 void testConstructor92() throws WrongSizeException {
9703 final var ba = generateBa();
9704 final var biasX = ba.getElementAtIndex(0);
9705 final var biasY = ba.getElementAtIndex(1);
9706 final var biasZ = ba.getElementAtIndex(2);
9707
9708 final var randomizer = new UniformRandomizer();
9709 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9710 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9711 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9712 final var nedPosition = new NEDPosition(latitude, longitude, height);
9713 final var nedVelocity = new NEDVelocity();
9714 final var ecefPosition = new ECEFPosition();
9715 final var ecefVelocity = new ECEFVelocity();
9716 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9717 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9718 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9719 final var gravityNorm = gravity.getNorm();
9720
9721 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
9722 biasX, biasY, biasZ, this);
9723
9724
9725 assertEquals(biasX, calibrator.getBiasX(), 0.0);
9726 assertEquals(biasY, calibrator.getBiasY(), 0.0);
9727 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9728 final var bx1 = calibrator.getBiasXAsAcceleration();
9729 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9730 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9731 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9732 calibrator.getBiasXAsAcceleration(bx2);
9733 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9734 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9735 final var by1 = calibrator.getBiasYAsAcceleration();
9736 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9737 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9738 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9739 calibrator.getBiasYAsAcceleration(by2);
9740 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9741 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9742 final var bz1 = calibrator.getBiasZAsAcceleration();
9743 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9744 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9745 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9746 calibrator.getBiasZAsAcceleration(bz2);
9747 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9748 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9749 final var biasTriad1 = calibrator.getBiasAsTriad();
9750 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9751 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9752 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9753 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9754 final var biasTriad2 = new AccelerationTriad();
9755 calibrator.getBiasAsTriad(biasTriad2);
9756 assertEquals(biasTriad1, biasTriad2);
9757 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9758 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9759 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9760 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9761 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9762 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9763 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9764 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9765 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9766 final var bias1 = calibrator.getBias();
9767 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9768 final var bias2 = new double[3];
9769 calibrator.getBias(bias2);
9770 assertArrayEquals(bias1, bias2, 0.0);
9771 final var b1 = calibrator.getBiasAsMatrix();
9772 assertEquals(b1, ba);
9773 final var b2 = new Matrix(3, 1);
9774 calibrator.getBiasAsMatrix(b2);
9775 assertEquals(b1, b2);
9776 final var ma1 = calibrator.getInitialMa();
9777 assertEquals(new Matrix(3, 3), ma1);
9778 final var ma2 = new Matrix(3, 3);
9779 calibrator.getInitialMa(ma2);
9780 assertEquals(ma1, ma2);
9781 assertNull(calibrator.getMeasurements());
9782 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9783 calibrator.getMeasurementType());
9784 assertFalse(calibrator.isOrderedMeasurementsRequired());
9785 assertFalse(calibrator.isQualityScoresRequired());
9786 assertTrue(calibrator.isCommonAxisUsed());
9787 assertSame(this, calibrator.getListener());
9788 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
9789 assertFalse(calibrator.isReady());
9790 assertFalse(calibrator.isRunning());
9791 assertNull(calibrator.getEstimatedMa());
9792 assertNull(calibrator.getEstimatedSx());
9793 assertNull(calibrator.getEstimatedSy());
9794 assertNull(calibrator.getEstimatedSz());
9795 assertNull(calibrator.getEstimatedMxy());
9796 assertNull(calibrator.getEstimatedMxz());
9797 assertNull(calibrator.getEstimatedMyx());
9798 assertNull(calibrator.getEstimatedMyz());
9799 assertNull(calibrator.getEstimatedMzx());
9800 assertNull(calibrator.getEstimatedMzy());
9801 assertNull(calibrator.getEstimatedCovariance());
9802 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9803 assertNotNull(calibrator.getGroundTruthGravityNorm());
9804 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9805 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9806 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9807 ABSOLUTE_ERROR));
9808 final var g = new Acceleration(0.0, AccelerationUnit.G);
9809 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9810 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9811 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9812
9813
9814 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
9815 -gravityNorm, true, biasX, biasY, biasZ, this));
9816 }
9817
9818 @Test
9819 void testConstructor93() throws WrongSizeException {
9820 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
9821
9822 final var ba = generateBa();
9823 final var biasX = ba.getElementAtIndex(0);
9824 final var biasY = ba.getElementAtIndex(1);
9825 final var biasZ = ba.getElementAtIndex(2);
9826
9827 final var randomizer = new UniformRandomizer();
9828 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9829 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9830 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9831 final var nedPosition = new NEDPosition(latitude, longitude, height);
9832 final var nedVelocity = new NEDVelocity();
9833 final var ecefPosition = new ECEFPosition();
9834 final var ecefVelocity = new ECEFVelocity();
9835 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9836 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9837 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9838 final var gravityNorm = gravity.getNorm();
9839
9840 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
9841 true, biasX, biasY, biasZ);
9842
9843
9844 assertEquals(biasX, calibrator.getBiasX(), 0.0);
9845 assertEquals(biasY, calibrator.getBiasY(), 0.0);
9846 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9847 final var bx1 = calibrator.getBiasXAsAcceleration();
9848 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9849 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9850 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9851 calibrator.getBiasXAsAcceleration(bx2);
9852 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9853 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9854 final var by1 = calibrator.getBiasYAsAcceleration();
9855 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9856 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9857 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9858 calibrator.getBiasYAsAcceleration(by2);
9859 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9860 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9861 final var bz1 = calibrator.getBiasZAsAcceleration();
9862 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9863 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9864 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9865 calibrator.getBiasZAsAcceleration(bz2);
9866 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9867 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9868 final var biasTriad1 = calibrator.getBiasAsTriad();
9869 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9870 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9871 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9872 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9873 final var biasTriad2 = new AccelerationTriad();
9874 calibrator.getBiasAsTriad(biasTriad2);
9875 assertEquals(biasTriad1, biasTriad2);
9876 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9877 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9878 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9879 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9880 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9881 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9882 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9883 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9884 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9885 final var bias1 = calibrator.getBias();
9886 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9887 final var bias2 = new double[3];
9888 calibrator.getBias(bias2);
9889 assertArrayEquals(bias1, bias2, 0.0);
9890 final var b1 = calibrator.getBiasAsMatrix();
9891 assertEquals(b1, ba);
9892 final var b2 = new Matrix(3, 1);
9893 calibrator.getBiasAsMatrix(b2);
9894 assertEquals(b1, b2);
9895 final var ma1 = calibrator.getInitialMa();
9896 assertEquals(new Matrix(3, 3), ma1);
9897 final var ma2 = new Matrix(3, 3);
9898 calibrator.getInitialMa(ma2);
9899 assertEquals(ma1, ma2);
9900 assertSame(measurements, calibrator.getMeasurements());
9901 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9902 calibrator.getMeasurementType());
9903 assertFalse(calibrator.isOrderedMeasurementsRequired());
9904 assertFalse(calibrator.isQualityScoresRequired());
9905 assertTrue(calibrator.isCommonAxisUsed());
9906 assertNull(calibrator.getListener());
9907 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
9908 assertFalse(calibrator.isReady());
9909 assertFalse(calibrator.isRunning());
9910 assertNull(calibrator.getEstimatedMa());
9911 assertNull(calibrator.getEstimatedSx());
9912 assertNull(calibrator.getEstimatedSy());
9913 assertNull(calibrator.getEstimatedSz());
9914 assertNull(calibrator.getEstimatedMxy());
9915 assertNull(calibrator.getEstimatedMxz());
9916 assertNull(calibrator.getEstimatedMyx());
9917 assertNull(calibrator.getEstimatedMyz());
9918 assertNull(calibrator.getEstimatedMzx());
9919 assertNull(calibrator.getEstimatedMzy());
9920 assertNull(calibrator.getEstimatedCovariance());
9921 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9922 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9923 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9924 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9925 ABSOLUTE_ERROR));
9926 final var g = new Acceleration(0.0, AccelerationUnit.G);
9927 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9928 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9929 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9930
9931
9932 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
9933 -gravityNorm, measurements, true, biasX, biasY, biasZ));
9934 }
9935
9936 @Test
9937 void testConstructor94() throws WrongSizeException {
9938 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
9939
9940 final var ba = generateBa();
9941 final var biasX = ba.getElementAtIndex(0);
9942 final var biasY = ba.getElementAtIndex(1);
9943 final var biasZ = ba.getElementAtIndex(2);
9944
9945 final var randomizer = new UniformRandomizer();
9946 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9947 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9948 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9949 final var nedPosition = new NEDPosition(latitude, longitude, height);
9950 final var nedVelocity = new NEDVelocity();
9951 final var ecefPosition = new ECEFPosition();
9952 final var ecefVelocity = new ECEFVelocity();
9953 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9954 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9955 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9956 final var gravityNorm = gravity.getNorm();
9957
9958 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
9959 true, biasX, biasY, biasZ, this);
9960
9961
9962 assertEquals(biasX, calibrator.getBiasX(), 0.0);
9963 assertEquals(biasY, calibrator.getBiasY(), 0.0);
9964 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9965 final var bx1 = calibrator.getBiasXAsAcceleration();
9966 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9967 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9968 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9969 calibrator.getBiasXAsAcceleration(bx2);
9970 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9971 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9972 final var by1 = calibrator.getBiasYAsAcceleration();
9973 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9974 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9975 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9976 calibrator.getBiasYAsAcceleration(by2);
9977 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9978 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9979 final var bz1 = calibrator.getBiasZAsAcceleration();
9980 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9981 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9982 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9983 calibrator.getBiasZAsAcceleration(bz2);
9984 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9985 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9986 final var biasTriad1 = calibrator.getBiasAsTriad();
9987 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9988 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9989 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9990 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9991 final var biasTriad2 = new AccelerationTriad();
9992 calibrator.getBiasAsTriad(biasTriad2);
9993 assertEquals(biasTriad1, biasTriad2);
9994 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9995 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9996 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9997 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9998 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9999 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10000 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10001 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10002 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10003 final var bias1 = calibrator.getBias();
10004 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10005 final var bias2 = new double[3];
10006 calibrator.getBias(bias2);
10007 assertArrayEquals(bias1, bias2, 0.0);
10008 final var b1 = calibrator.getBiasAsMatrix();
10009 assertEquals(b1, ba);
10010 final var b2 = new Matrix(3, 1);
10011 calibrator.getBiasAsMatrix(b2);
10012 assertEquals(b1, b2);
10013 final var ma1 = calibrator.getInitialMa();
10014 assertEquals(new Matrix(3, 3), ma1);
10015 final var ma2 = new Matrix(3, 3);
10016 calibrator.getInitialMa(ma2);
10017 assertEquals(ma1, ma2);
10018 assertSame(measurements, calibrator.getMeasurements());
10019 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10020 calibrator.getMeasurementType());
10021 assertFalse(calibrator.isOrderedMeasurementsRequired());
10022 assertFalse(calibrator.isQualityScoresRequired());
10023 assertTrue(calibrator.isCommonAxisUsed());
10024 assertSame(this, calibrator.getListener());
10025 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
10026 assertFalse(calibrator.isReady());
10027 assertFalse(calibrator.isRunning());
10028 assertNull(calibrator.getEstimatedMa());
10029 assertNull(calibrator.getEstimatedSx());
10030 assertNull(calibrator.getEstimatedSy());
10031 assertNull(calibrator.getEstimatedSz());
10032 assertNull(calibrator.getEstimatedMxy());
10033 assertNull(calibrator.getEstimatedMxz());
10034 assertNull(calibrator.getEstimatedMyx());
10035 assertNull(calibrator.getEstimatedMyz());
10036 assertNull(calibrator.getEstimatedMzx());
10037 assertNull(calibrator.getEstimatedMzy());
10038 assertNull(calibrator.getEstimatedCovariance());
10039 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10040 assertNotNull(calibrator.getGroundTruthGravityNorm());
10041 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10042 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10043 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10044 ABSOLUTE_ERROR));
10045 final var g = new Acceleration(0.0, AccelerationUnit.G);
10046 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10047 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10048 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10049
10050
10051 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10052 -gravityNorm, measurements, true, biasX, biasY, biasZ, this));
10053 }
10054
10055 @Test
10056 void testConstructor95() throws WrongSizeException {
10057 final var ba = generateBa();
10058 final var biasX = ba.getElementAtIndex(0);
10059 final var biasY = ba.getElementAtIndex(1);
10060 final var biasZ = ba.getElementAtIndex(2);
10061
10062 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10063 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10064 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10065
10066 final var randomizer = new UniformRandomizer();
10067 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10068 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10069 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10070 final var nedPosition = new NEDPosition(latitude, longitude, height);
10071 final var nedVelocity = new NEDVelocity();
10072 final var ecefPosition = new ECEFPosition();
10073 final var ecefVelocity = new ECEFVelocity();
10074 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10075 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10076 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10077 final var gravityNorm = gravity.getNorm();
10078
10079 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz);
10080
10081
10082 assertEquals(biasX, calibrator.getBiasX(), 0.0);
10083 assertEquals(biasY, calibrator.getBiasY(), 0.0);
10084 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10085 final var bx1 = calibrator.getBiasXAsAcceleration();
10086 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10087 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10088 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10089 calibrator.getBiasXAsAcceleration(bx2);
10090 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10091 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10092 final var by1 = calibrator.getBiasYAsAcceleration();
10093 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
10094 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10095 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10096 calibrator.getBiasYAsAcceleration(by2);
10097 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10098 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10099 final var bz1 = calibrator.getBiasZAsAcceleration();
10100 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10101 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10102 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10103 calibrator.getBiasZAsAcceleration(bz2);
10104 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10105 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10106 final var biasTriad1 = calibrator.getBiasAsTriad();
10107 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10108 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10109 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10110 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10111 final var biasTriad2 = new AccelerationTriad();
10112 calibrator.getBiasAsTriad(biasTriad2);
10113 assertEquals(biasTriad1, biasTriad2);
10114 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10115 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10116 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10117 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10118 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10119 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10120 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10121 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10122 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10123 final var bias1 = calibrator.getBias();
10124 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10125 final var bias2 = new double[3];
10126 calibrator.getBias(bias2);
10127 assertArrayEquals(bias1, bias2, 0.0);
10128 final var b1 = calibrator.getBiasAsMatrix();
10129 assertEquals(b1, ba);
10130 final var b2 = new Matrix(3, 1);
10131 calibrator.getBiasAsMatrix(b2);
10132 assertEquals(b1, b2);
10133 final var ma1 = calibrator.getInitialMa();
10134 assertEquals(new Matrix(3, 3), ma1);
10135 final var ma2 = new Matrix(3, 3);
10136 calibrator.getInitialMa(ma2);
10137 assertEquals(ma1, ma2);
10138 assertNull(calibrator.getMeasurements());
10139 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10140 calibrator.getMeasurementType());
10141 assertFalse(calibrator.isOrderedMeasurementsRequired());
10142 assertFalse(calibrator.isQualityScoresRequired());
10143 assertFalse(calibrator.isCommonAxisUsed());
10144 assertNull(calibrator.getListener());
10145 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
10146 assertFalse(calibrator.isReady());
10147 assertFalse(calibrator.isRunning());
10148 assertNull(calibrator.getEstimatedMa());
10149 assertNull(calibrator.getEstimatedSx());
10150 assertNull(calibrator.getEstimatedSy());
10151 assertNull(calibrator.getEstimatedSz());
10152 assertNull(calibrator.getEstimatedMxy());
10153 assertNull(calibrator.getEstimatedMxz());
10154 assertNull(calibrator.getEstimatedMyx());
10155 assertNull(calibrator.getEstimatedMyz());
10156 assertNull(calibrator.getEstimatedMzx());
10157 assertNull(calibrator.getEstimatedMzy());
10158 assertNull(calibrator.getEstimatedCovariance());
10159 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10160 assertNotNull(calibrator.getGroundTruthGravityNorm());
10161 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10162 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10163 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10164 ABSOLUTE_ERROR));
10165 final var g = new Acceleration(0.0, AccelerationUnit.G);
10166 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10167 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10168 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10169
10170
10171 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10172 -gravityNorm, bx, by, bz));
10173 }
10174
10175 @Test
10176 void testConstructor96() throws WrongSizeException {
10177 final var ba = generateBa();
10178 final var biasX = ba.getElementAtIndex(0);
10179 final var biasY = ba.getElementAtIndex(1);
10180 final var biasZ = ba.getElementAtIndex(2);
10181
10182 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10183 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10184 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10185
10186 final var randomizer = new UniformRandomizer();
10187 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10188 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10189 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10190 final var nedPosition = new NEDPosition(latitude, longitude, height);
10191 final var nedVelocity = new NEDVelocity();
10192 final var ecefPosition = new ECEFPosition();
10193 final var ecefVelocity = new ECEFVelocity();
10194 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10195 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10196 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10197 final var gravityNorm = gravity.getNorm();
10198
10199 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz,
10200 this);
10201
10202
10203 assertEquals(biasX, calibrator.getBiasX(), 0.0);
10204 assertEquals(biasY, calibrator.getBiasY(), 0.0);
10205 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10206 final var bx1 = calibrator.getBiasXAsAcceleration();
10207 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10208 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10209 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10210 calibrator.getBiasXAsAcceleration(bx2);
10211 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10212 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10213 final var by1 = calibrator.getBiasYAsAcceleration();
10214 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10215 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10216 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10217 calibrator.getBiasYAsAcceleration(by2);
10218 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10219 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10220 final var bz1 = calibrator.getBiasZAsAcceleration();
10221 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10222 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10223 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10224 calibrator.getBiasZAsAcceleration(bz2);
10225 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10226 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10227 final var biasTriad1 = calibrator.getBiasAsTriad();
10228 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10229 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10230 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10231 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10232 final var biasTriad2 = new AccelerationTriad();
10233 calibrator.getBiasAsTriad(biasTriad2);
10234 assertEquals(biasTriad1, biasTriad2);
10235 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10236 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10237 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10238 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10239 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10240 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10241 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10242 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10243 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10244 final var bias1 = calibrator.getBias();
10245 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10246 final var bias2 = new double[3];
10247 calibrator.getBias(bias2);
10248 assertArrayEquals(bias1, bias2, 0.0);
10249 final var b1 = calibrator.getBiasAsMatrix();
10250 assertEquals(b1, ba);
10251 final var b2 = new Matrix(3, 1);
10252 calibrator.getBiasAsMatrix(b2);
10253 assertEquals(b1, b2);
10254 final var ma1 = calibrator.getInitialMa();
10255 assertEquals(new Matrix(3, 3), ma1);
10256 final var ma2 = new Matrix(3, 3);
10257 calibrator.getInitialMa(ma2);
10258 assertEquals(ma1, ma2);
10259 assertNull(calibrator.getMeasurements());
10260 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10261 calibrator.getMeasurementType());
10262 assertFalse(calibrator.isOrderedMeasurementsRequired());
10263 assertFalse(calibrator.isQualityScoresRequired());
10264 assertFalse(calibrator.isCommonAxisUsed());
10265 assertSame(this, calibrator.getListener());
10266 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
10267 assertFalse(calibrator.isReady());
10268 assertFalse(calibrator.isRunning());
10269 assertNull(calibrator.getEstimatedMa());
10270 assertNull(calibrator.getEstimatedSx());
10271 assertNull(calibrator.getEstimatedSy());
10272 assertNull(calibrator.getEstimatedSz());
10273 assertNull(calibrator.getEstimatedMxy());
10274 assertNull(calibrator.getEstimatedMxz());
10275 assertNull(calibrator.getEstimatedMyx());
10276 assertNull(calibrator.getEstimatedMyz());
10277 assertNull(calibrator.getEstimatedMzx());
10278 assertNull(calibrator.getEstimatedMzy());
10279 assertNull(calibrator.getEstimatedCovariance());
10280 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10281 assertNotNull(calibrator.getGroundTruthGravityNorm());
10282 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10283 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10284 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10285 ABSOLUTE_ERROR));
10286 final var g = new Acceleration(0.0, AccelerationUnit.G);
10287 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10288 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10289 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10290
10291
10292 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10293 -gravityNorm, bx, by, bz, this));
10294 }
10295
10296 @Test
10297 void testConstructor97() throws WrongSizeException {
10298 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
10299
10300 final var ba = generateBa();
10301 final var biasX = ba.getElementAtIndex(0);
10302 final var biasY = ba.getElementAtIndex(1);
10303 final var biasZ = ba.getElementAtIndex(2);
10304
10305 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10306 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10307 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10308
10309 final var randomizer = new UniformRandomizer();
10310 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10311 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10312 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10313 final var nedPosition = new NEDPosition(latitude, longitude, height);
10314 final var nedVelocity = new NEDVelocity();
10315 final var ecefPosition = new ECEFPosition();
10316 final var ecefVelocity = new ECEFVelocity();
10317 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10318 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10319 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10320 final var gravityNorm = gravity.getNorm();
10321
10322 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
10323 bx, by, bz);
10324
10325
10326 assertEquals(biasX, calibrator.getBiasX(), 0.0);
10327 assertEquals(biasY, calibrator.getBiasY(), 0.0);
10328 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10329 final var bx1 = calibrator.getBiasXAsAcceleration();
10330 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10331 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10332 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10333 calibrator.getBiasXAsAcceleration(bx2);
10334 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10335 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10336 final var by1 = calibrator.getBiasYAsAcceleration();
10337 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
10338 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10339 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10340 calibrator.getBiasYAsAcceleration(by2);
10341 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10342 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10343 final var bz1 = calibrator.getBiasZAsAcceleration();
10344 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10345 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10346 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10347 calibrator.getBiasZAsAcceleration(bz2);
10348 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10349 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10350 final var biasTriad1 = calibrator.getBiasAsTriad();
10351 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10352 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10353 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10354 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10355 final var biasTriad2 = new AccelerationTriad();
10356 calibrator.getBiasAsTriad(biasTriad2);
10357 assertEquals(biasTriad1, biasTriad2);
10358 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10359 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10360 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10361 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10362 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10363 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10364 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10365 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10366 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10367 final var bias1 = calibrator.getBias();
10368 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10369 final var bias2 = new double[3];
10370 calibrator.getBias(bias2);
10371 assertArrayEquals(bias1, bias2, 0.0);
10372 final var b1 = calibrator.getBiasAsMatrix();
10373 assertEquals(b1, ba);
10374 final var b2 = new Matrix(3, 1);
10375 calibrator.getBiasAsMatrix(b2);
10376 assertEquals(b1, b2);
10377 final var ma1 = calibrator.getInitialMa();
10378 assertEquals(new Matrix(3, 3), ma1);
10379 final var ma2 = new Matrix(3, 3);
10380 calibrator.getInitialMa(ma2);
10381 assertEquals(ma1, ma2);
10382 assertSame(measurements, calibrator.getMeasurements());
10383 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10384 calibrator.getMeasurementType());
10385 assertFalse(calibrator.isOrderedMeasurementsRequired());
10386 assertFalse(calibrator.isQualityScoresRequired());
10387 assertFalse(calibrator.isCommonAxisUsed());
10388 assertNull(calibrator.getListener());
10389 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
10390 assertFalse(calibrator.isReady());
10391 assertFalse(calibrator.isRunning());
10392 assertNull(calibrator.getEstimatedMa());
10393 assertNull(calibrator.getEstimatedSx());
10394 assertNull(calibrator.getEstimatedSy());
10395 assertNull(calibrator.getEstimatedSz());
10396 assertNull(calibrator.getEstimatedMxy());
10397 assertNull(calibrator.getEstimatedMxz());
10398 assertNull(calibrator.getEstimatedMyx());
10399 assertNull(calibrator.getEstimatedMyz());
10400 assertNull(calibrator.getEstimatedMzx());
10401 assertNull(calibrator.getEstimatedMzy());
10402 assertNull(calibrator.getEstimatedCovariance());
10403 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10404 assertNotNull(calibrator.getGroundTruthGravityNorm());
10405 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10406 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10407 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10408 ABSOLUTE_ERROR));
10409 final var g = new Acceleration(0.0, AccelerationUnit.G);
10410 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10411 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10412 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10413
10414
10415 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10416 -gravityNorm, measurements, bx, by, bz));
10417 }
10418
10419 @Test
10420 void testConstructor98() throws WrongSizeException {
10421 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
10422
10423 final var ba = generateBa();
10424 final var biasX = ba.getElementAtIndex(0);
10425 final var biasY = ba.getElementAtIndex(1);
10426 final var biasZ = ba.getElementAtIndex(2);
10427
10428 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10429 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10430 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10431
10432 final var randomizer = new UniformRandomizer();
10433 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10434 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10435 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10436 final var nedPosition = new NEDPosition(latitude, longitude, height);
10437 final var nedVelocity = new NEDVelocity();
10438 final var ecefPosition = new ECEFPosition();
10439 final var ecefVelocity = new ECEFVelocity();
10440 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10441 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10442 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10443 final var gravityNorm = gravity.getNorm();
10444
10445 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
10446 this);
10447
10448
10449 assertEquals(biasX, calibrator.getBiasX(), 0.0);
10450 assertEquals(biasY, calibrator.getBiasY(), 0.0);
10451 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10452 final var bx1 = calibrator.getBiasXAsAcceleration();
10453 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10454 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10455 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10456 calibrator.getBiasXAsAcceleration(bx2);
10457 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10458 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10459 final var by1 = calibrator.getBiasYAsAcceleration();
10460 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
10461 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10462 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10463 calibrator.getBiasYAsAcceleration(by2);
10464 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10465 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10466 final var bz1 = calibrator.getBiasZAsAcceleration();
10467 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10468 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10469 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10470 calibrator.getBiasZAsAcceleration(bz2);
10471 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10472 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10473 final var biasTriad1 = calibrator.getBiasAsTriad();
10474 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10475 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10476 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10477 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10478 final var biasTriad2 = new AccelerationTriad();
10479 calibrator.getBiasAsTriad(biasTriad2);
10480 assertEquals(biasTriad1, biasTriad2);
10481 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10482 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10483 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10484 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10485 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10486 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10487 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10488 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10489 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10490 final var bias1 = calibrator.getBias();
10491 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10492 final var bias2 = new double[3];
10493 calibrator.getBias(bias2);
10494 assertArrayEquals(bias1, bias2, 0.0);
10495 final var b1 = calibrator.getBiasAsMatrix();
10496 assertEquals(b1, ba);
10497 final var b2 = new Matrix(3, 1);
10498 calibrator.getBiasAsMatrix(b2);
10499 assertEquals(b1, b2);
10500 final var ma1 = calibrator.getInitialMa();
10501 assertEquals(new Matrix(3, 3), ma1);
10502 final var ma2 = new Matrix(3, 3);
10503 calibrator.getInitialMa(ma2);
10504 assertEquals(ma1, ma2);
10505 assertSame(measurements, calibrator.getMeasurements());
10506 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10507 calibrator.getMeasurementType());
10508 assertFalse(calibrator.isOrderedMeasurementsRequired());
10509 assertFalse(calibrator.isQualityScoresRequired());
10510 assertFalse(calibrator.isCommonAxisUsed());
10511 assertSame(this, calibrator.getListener());
10512 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
10513 assertFalse(calibrator.isReady());
10514 assertFalse(calibrator.isRunning());
10515 assertNull(calibrator.getEstimatedMa());
10516 assertNull(calibrator.getEstimatedSx());
10517 assertNull(calibrator.getEstimatedSy());
10518 assertNull(calibrator.getEstimatedSz());
10519 assertNull(calibrator.getEstimatedMxy());
10520 assertNull(calibrator.getEstimatedMxz());
10521 assertNull(calibrator.getEstimatedMyx());
10522 assertNull(calibrator.getEstimatedMyz());
10523 assertNull(calibrator.getEstimatedMzx());
10524 assertNull(calibrator.getEstimatedMzy());
10525 assertNull(calibrator.getEstimatedCovariance());
10526 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10527 assertNotNull(calibrator.getGroundTruthGravityNorm());
10528 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10529 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10530 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10531 ABSOLUTE_ERROR));
10532 final var g = new Acceleration(0.0, AccelerationUnit.G);
10533 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10534 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10535 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10536
10537
10538 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10539 -gravityNorm, measurements, bx, by, bz, this));
10540 }
10541
10542 @Test
10543 void testConstructor99() throws WrongSizeException {
10544 final var ba = generateBa();
10545 final var biasX = ba.getElementAtIndex(0);
10546 final var biasY = ba.getElementAtIndex(1);
10547 final var biasZ = ba.getElementAtIndex(2);
10548
10549 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10550 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10551 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10552
10553 final var randomizer = new UniformRandomizer();
10554 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10555 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10556 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10557 final var nedPosition = new NEDPosition(latitude, longitude, height);
10558 final var nedVelocity = new NEDVelocity();
10559 final var ecefPosition = new ECEFPosition();
10560 final var ecefVelocity = new ECEFVelocity();
10561 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10562 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10563 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10564 final var gravityNorm = gravity.getNorm();
10565
10566 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
10567 bx, by, bz);
10568
10569
10570 assertEquals(biasX, calibrator.getBiasX(), 0.0);
10571 assertEquals(biasY, calibrator.getBiasY(), 0.0);
10572 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10573 final var bx1 = calibrator.getBiasXAsAcceleration();
10574 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10575 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10576 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10577 calibrator.getBiasXAsAcceleration(bx2);
10578 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10579 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10580 final var by1 = calibrator.getBiasYAsAcceleration();
10581 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
10582 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10583 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10584 calibrator.getBiasYAsAcceleration(by2);
10585 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10586 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10587 final var bz1 = calibrator.getBiasZAsAcceleration();
10588 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10589 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10590 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10591 calibrator.getBiasZAsAcceleration(bz2);
10592 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10593 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10594 final var biasTriad1 = calibrator.getBiasAsTriad();
10595 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10596 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10597 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10598 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10599 final var biasTriad2 = new AccelerationTriad();
10600 calibrator.getBiasAsTriad(biasTriad2);
10601 assertEquals(biasTriad1, biasTriad2);
10602 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10603 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10604 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10605 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10606 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10607 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10608 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10609 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10610 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10611 final var bias1 = calibrator.getBias();
10612 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10613 final var bias2 = new double[3];
10614 calibrator.getBias(bias2);
10615 assertArrayEquals(bias1, bias2, 0.0);
10616 final var b1 = calibrator.getBiasAsMatrix();
10617 assertEquals(b1, ba);
10618 final var b2 = new Matrix(3, 1);
10619 calibrator.getBiasAsMatrix(b2);
10620 assertEquals(b1, b2);
10621 final var ma1 = calibrator.getInitialMa();
10622 assertEquals(new Matrix(3, 3), ma1);
10623 final var ma2 = new Matrix(3, 3);
10624 calibrator.getInitialMa(ma2);
10625 assertEquals(ma1, ma2);
10626 assertNull(calibrator.getMeasurements());
10627 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10628 calibrator.getMeasurementType());
10629 assertFalse(calibrator.isOrderedMeasurementsRequired());
10630 assertFalse(calibrator.isQualityScoresRequired());
10631 assertTrue(calibrator.isCommonAxisUsed());
10632 assertNull(calibrator.getListener());
10633 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
10634 assertFalse(calibrator.isReady());
10635 assertFalse(calibrator.isRunning());
10636 assertNull(calibrator.getEstimatedMa());
10637 assertNull(calibrator.getEstimatedSx());
10638 assertNull(calibrator.getEstimatedSy());
10639 assertNull(calibrator.getEstimatedSz());
10640 assertNull(calibrator.getEstimatedMxy());
10641 assertNull(calibrator.getEstimatedMxz());
10642 assertNull(calibrator.getEstimatedMyx());
10643 assertNull(calibrator.getEstimatedMyz());
10644 assertNull(calibrator.getEstimatedMzx());
10645 assertNull(calibrator.getEstimatedMzy());
10646 assertNull(calibrator.getEstimatedCovariance());
10647 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10648 assertNotNull(calibrator.getGroundTruthGravityNorm());
10649 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10650 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10651 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10652 ABSOLUTE_ERROR));
10653 final var g = new Acceleration(0.0, AccelerationUnit.G);
10654 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10655 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10656 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10657
10658
10659 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10660 -gravityNorm, true, bx, by, bz));
10661 }
10662
10663 @Test
10664 void testConstructor100() throws WrongSizeException {
10665 final var ba = generateBa();
10666 final var biasX = ba.getElementAtIndex(0);
10667 final var biasY = ba.getElementAtIndex(1);
10668 final var biasZ = ba.getElementAtIndex(2);
10669
10670 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10671 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10672 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10673
10674 final var randomizer = new UniformRandomizer();
10675 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10676 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10677 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10678 final var nedPosition = new NEDPosition(latitude, longitude, height);
10679 final var nedVelocity = new NEDVelocity();
10680 final var ecefPosition = new ECEFPosition();
10681 final var ecefVelocity = new ECEFVelocity();
10682 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10683 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10684 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10685 final var gravityNorm = gravity.getNorm();
10686
10687 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
10688 bx, by, bz, this);
10689
10690
10691 assertEquals(biasX, calibrator.getBiasX(), 0.0);
10692 assertEquals(biasY, calibrator.getBiasY(), 0.0);
10693 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10694 final var bx1 = calibrator.getBiasXAsAcceleration();
10695 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10696 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10697 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10698 calibrator.getBiasXAsAcceleration(bx2);
10699 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10700 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10701 final var by1 = calibrator.getBiasYAsAcceleration();
10702 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
10703 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10704 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10705 calibrator.getBiasYAsAcceleration(by2);
10706 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10707 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10708 final var bz1 = calibrator.getBiasZAsAcceleration();
10709 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10710 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10711 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10712 calibrator.getBiasZAsAcceleration(bz2);
10713 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10714 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10715 final var biasTriad1 = calibrator.getBiasAsTriad();
10716 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10717 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10718 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10719 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10720 final var biasTriad2 = new AccelerationTriad();
10721 calibrator.getBiasAsTriad(biasTriad2);
10722 assertEquals(biasTriad1, biasTriad2);
10723 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10724 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10725 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10726 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10727 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10728 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10729 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10730 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10731 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10732 final var bias1 = calibrator.getBias();
10733 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10734 final var bias2 = new double[3];
10735 calibrator.getBias(bias2);
10736 assertArrayEquals(bias1, bias2, 0.0);
10737 final var b1 = calibrator.getBiasAsMatrix();
10738 assertEquals(b1, ba);
10739 final var b2 = new Matrix(3, 1);
10740 calibrator.getBiasAsMatrix(b2);
10741 assertEquals(b1, b2);
10742 final var ma1 = calibrator.getInitialMa();
10743 assertEquals(new Matrix(3, 3), ma1);
10744 final var ma2 = new Matrix(3, 3);
10745 calibrator.getInitialMa(ma2);
10746 assertEquals(ma1, ma2);
10747 assertNull(calibrator.getMeasurements());
10748 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10749 calibrator.getMeasurementType());
10750 assertFalse(calibrator.isOrderedMeasurementsRequired());
10751 assertFalse(calibrator.isQualityScoresRequired());
10752 assertTrue(calibrator.isCommonAxisUsed());
10753 assertSame(this, calibrator.getListener());
10754 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
10755 assertFalse(calibrator.isReady());
10756 assertFalse(calibrator.isRunning());
10757 assertNull(calibrator.getEstimatedMa());
10758 assertNull(calibrator.getEstimatedSx());
10759 assertNull(calibrator.getEstimatedSy());
10760 assertNull(calibrator.getEstimatedSz());
10761 assertNull(calibrator.getEstimatedMxy());
10762 assertNull(calibrator.getEstimatedMxz());
10763 assertNull(calibrator.getEstimatedMyx());
10764 assertNull(calibrator.getEstimatedMyz());
10765 assertNull(calibrator.getEstimatedMzx());
10766 assertNull(calibrator.getEstimatedMzy());
10767 assertNull(calibrator.getEstimatedCovariance());
10768 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10769 assertNotNull(calibrator.getGroundTruthGravityNorm());
10770 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10771 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10772 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10773 ABSOLUTE_ERROR));
10774 final var g = new Acceleration(0.0, AccelerationUnit.G);
10775 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10776 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10777 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10778
10779
10780 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10781 -gravityNorm, true, bx, by, bz, this));
10782 }
10783
10784 @Test
10785 void testConstructor101() throws WrongSizeException {
10786 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
10787
10788 final var ba = generateBa();
10789 final var biasX = ba.getElementAtIndex(0);
10790 final var biasY = ba.getElementAtIndex(1);
10791 final var biasZ = ba.getElementAtIndex(2);
10792
10793 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10794 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10795 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10796
10797 final var randomizer = new UniformRandomizer();
10798 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10799 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10800 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10801 final var nedPosition = new NEDPosition(latitude, longitude, height);
10802 final var nedVelocity = new NEDVelocity();
10803 final var ecefPosition = new ECEFPosition();
10804 final var ecefVelocity = new ECEFVelocity();
10805 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10806 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10807 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10808 final var gravityNorm = gravity.getNorm();
10809
10810 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
10811 true, bx, by, bz);
10812
10813
10814 assertEquals(biasX, calibrator.getBiasX(), 0.0);
10815 assertEquals(biasY, calibrator.getBiasY(), 0.0);
10816 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10817 final var bx1 = calibrator.getBiasXAsAcceleration();
10818 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10819 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10820 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10821 calibrator.getBiasXAsAcceleration(bx2);
10822 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10823 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10824 final var by1 = calibrator.getBiasYAsAcceleration();
10825 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
10826 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10827 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10828 calibrator.getBiasYAsAcceleration(by2);
10829 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10830 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10831 final var bz1 = calibrator.getBiasZAsAcceleration();
10832 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10833 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10834 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10835 calibrator.getBiasZAsAcceleration(bz2);
10836 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10837 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10838 final var biasTriad1 = calibrator.getBiasAsTriad();
10839 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10840 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10841 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10842 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10843 final var biasTriad2 = new AccelerationTriad();
10844 calibrator.getBiasAsTriad(biasTriad2);
10845 assertEquals(biasTriad1, biasTriad2);
10846 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10847 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10848 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10849 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10850 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10851 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10852 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10853 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10854 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10855 final var bias1 = calibrator.getBias();
10856 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10857 final var bias2 = new double[3];
10858 calibrator.getBias(bias2);
10859 assertArrayEquals(bias1, bias2, 0.0);
10860 final var b1 = calibrator.getBiasAsMatrix();
10861 assertEquals(b1, ba);
10862 final var b2 = new Matrix(3, 1);
10863 calibrator.getBiasAsMatrix(b2);
10864 assertEquals(b1, b2);
10865 final var ma1 = calibrator.getInitialMa();
10866 assertEquals(new Matrix(3, 3), ma1);
10867 final var ma2 = new Matrix(3, 3);
10868 calibrator.getInitialMa(ma2);
10869 assertEquals(ma1, ma2);
10870 assertSame(measurements, calibrator.getMeasurements());
10871 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10872 calibrator.getMeasurementType());
10873 assertFalse(calibrator.isOrderedMeasurementsRequired());
10874 assertFalse(calibrator.isQualityScoresRequired());
10875 assertTrue(calibrator.isCommonAxisUsed());
10876 assertNull(calibrator.getListener());
10877 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
10878 assertFalse(calibrator.isReady());
10879 assertFalse(calibrator.isRunning());
10880 assertNull(calibrator.getEstimatedMa());
10881 assertNull(calibrator.getEstimatedSx());
10882 assertNull(calibrator.getEstimatedSy());
10883 assertNull(calibrator.getEstimatedSz());
10884 assertNull(calibrator.getEstimatedMxy());
10885 assertNull(calibrator.getEstimatedMxz());
10886 assertNull(calibrator.getEstimatedMyx());
10887 assertNull(calibrator.getEstimatedMyz());
10888 assertNull(calibrator.getEstimatedMzx());
10889 assertNull(calibrator.getEstimatedMzy());
10890 assertNull(calibrator.getEstimatedCovariance());
10891 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10892 assertNotNull(calibrator.getGroundTruthGravityNorm());
10893 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10894 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10895 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10896 ABSOLUTE_ERROR));
10897 final var g = new Acceleration(0.0, AccelerationUnit.G);
10898 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10899 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10900 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10901
10902
10903 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10904 -gravityNorm, measurements, true, bx, by, bz));
10905 }
10906
10907 @Test
10908 void testConstructor102() throws WrongSizeException {
10909 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
10910
10911 final var ba = generateBa();
10912 final var biasX = ba.getElementAtIndex(0);
10913 final var biasY = ba.getElementAtIndex(1);
10914 final var biasZ = ba.getElementAtIndex(2);
10915
10916 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10917 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10918 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10919
10920 final var randomizer = new UniformRandomizer();
10921 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10922 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10923 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10924 final var nedPosition = new NEDPosition(latitude, longitude, height);
10925 final var nedVelocity = new NEDVelocity();
10926 final var ecefPosition = new ECEFPosition();
10927 final var ecefVelocity = new ECEFVelocity();
10928 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10929 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10930 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10931 final var gravityNorm = gravity.getNorm();
10932
10933 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
10934 true, bx, by, bz, this);
10935
10936
10937 assertEquals(biasX, calibrator.getBiasX(), 0.0);
10938 assertEquals(biasY, calibrator.getBiasY(), 0.0);
10939 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10940 final var bx1 = calibrator.getBiasXAsAcceleration();
10941 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10942 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10943 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10944 calibrator.getBiasXAsAcceleration(bx2);
10945 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10946 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10947 final var by1 = calibrator.getBiasYAsAcceleration();
10948 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
10949 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10950 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10951 calibrator.getBiasYAsAcceleration(by2);
10952 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10953 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10954 final var bz1 = calibrator.getBiasZAsAcceleration();
10955 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10956 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10957 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10958 calibrator.getBiasZAsAcceleration(bz2);
10959 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10960 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10961 final var biasTriad1 = calibrator.getBiasAsTriad();
10962 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10963 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10964 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10965 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10966 final var biasTriad2 = new AccelerationTriad();
10967 calibrator.getBiasAsTriad(biasTriad2);
10968 assertEquals(biasTriad1, biasTriad2);
10969 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10970 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10971 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10972 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10973 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10974 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10975 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10976 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10977 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10978 final var bias1 = calibrator.getBias();
10979 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10980 final var bias2 = new double[3];
10981 calibrator.getBias(bias2);
10982 assertArrayEquals(bias1, bias2, 0.0);
10983 final var b1 = calibrator.getBiasAsMatrix();
10984 assertEquals(b1, ba);
10985 final var b2 = new Matrix(3, 1);
10986 calibrator.getBiasAsMatrix(b2);
10987 assertEquals(b1, b2);
10988 final var ma1 = calibrator.getInitialMa();
10989 assertEquals(new Matrix(3, 3), ma1);
10990 final var ma2 = new Matrix(3, 3);
10991 calibrator.getInitialMa(ma2);
10992 assertEquals(ma1, ma2);
10993 assertSame(measurements, calibrator.getMeasurements());
10994 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10995 calibrator.getMeasurementType());
10996 assertFalse(calibrator.isOrderedMeasurementsRequired());
10997 assertFalse(calibrator.isQualityScoresRequired());
10998 assertTrue(calibrator.isCommonAxisUsed());
10999 assertSame(this, calibrator.getListener());
11000 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
11001 assertFalse(calibrator.isReady());
11002 assertFalse(calibrator.isRunning());
11003 assertNull(calibrator.getEstimatedMa());
11004 assertNull(calibrator.getEstimatedSx());
11005 assertNull(calibrator.getEstimatedSy());
11006 assertNull(calibrator.getEstimatedSz());
11007 assertNull(calibrator.getEstimatedMxy());
11008 assertNull(calibrator.getEstimatedMxz());
11009 assertNull(calibrator.getEstimatedMyx());
11010 assertNull(calibrator.getEstimatedMyz());
11011 assertNull(calibrator.getEstimatedMzx());
11012 assertNull(calibrator.getEstimatedMzy());
11013 assertNull(calibrator.getEstimatedCovariance());
11014 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11015 assertNotNull(calibrator.getGroundTruthGravityNorm());
11016 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11017 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11018 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11019 ABSOLUTE_ERROR));
11020 final var g = new Acceleration(0.0, AccelerationUnit.G);
11021 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11022 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11023 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11024
11025
11026 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11027 -gravityNorm, measurements, true, bx, by, bz, this));
11028 }
11029
11030 @Test
11031 void testConstructor103() throws WrongSizeException {
11032 final var ba = generateBa();
11033 final var biasX = ba.getElementAtIndex(0);
11034 final var biasY = ba.getElementAtIndex(1);
11035 final var biasZ = ba.getElementAtIndex(2);
11036
11037 final var ma = generateMaCommonAxis();
11038 final var sx = ma.getElementAt(0, 0);
11039 final var sy = ma.getElementAt(1, 1);
11040 final var sz = ma.getElementAt(2, 2);
11041
11042 final var randomizer = new UniformRandomizer();
11043 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11044 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11045 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11046 final var nedPosition = new NEDPosition(latitude, longitude, height);
11047 final var nedVelocity = new NEDVelocity();
11048 final var ecefPosition = new ECEFPosition();
11049 final var ecefVelocity = new ECEFVelocity();
11050 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11051 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11052 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11053 final var gravityNorm = gravity.getNorm();
11054
11055 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ,
11056 sx, sy, sz);
11057
11058
11059 assertEquals(biasX, calibrator.getBiasX(), 0.0);
11060 assertEquals(biasY, calibrator.getBiasY(), 0.0);
11061 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11062 final var bx1 = calibrator.getBiasXAsAcceleration();
11063 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11064 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11065 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11066 calibrator.getBiasXAsAcceleration(bx2);
11067 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11068 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11069 final var by1 = calibrator.getBiasYAsAcceleration();
11070 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11071 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11072 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11073 calibrator.getBiasYAsAcceleration(by2);
11074 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11075 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11076 final var bz1 = calibrator.getBiasZAsAcceleration();
11077 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11078 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11079 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11080 calibrator.getBiasZAsAcceleration(bz2);
11081 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11082 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11083 final var biasTriad1 = calibrator.getBiasAsTriad();
11084 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11085 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11086 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11087 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11088 final var biasTriad2 = new AccelerationTriad();
11089 calibrator.getBiasAsTriad(biasTriad2);
11090 assertEquals(biasTriad1, biasTriad2);
11091 assertEquals(sx, calibrator.getInitialSx(), 0.0);
11092 assertEquals(sy, calibrator.getInitialSy(), 0.0);
11093 assertEquals(sz, calibrator.getInitialSz(), 0.0);
11094 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11095 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11096 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11097 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11098 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11099 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11100 final var bias1 = calibrator.getBias();
11101 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11102 final var bias2 = new double[3];
11103 calibrator.getBias(bias2);
11104 assertArrayEquals(bias1, bias2, 0.0);
11105 final var b1 = calibrator.getBiasAsMatrix();
11106 assertEquals(b1, ba);
11107 final var b2 = new Matrix(3, 1);
11108 calibrator.getBiasAsMatrix(b2);
11109 assertEquals(b1, b2);
11110 final var ma1 = calibrator.getInitialMa();
11111 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11112 final var ma2 = new Matrix(3, 3);
11113 calibrator.getInitialMa(ma2);
11114 assertEquals(ma1, ma2);
11115 assertNull(calibrator.getMeasurements());
11116 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11117 calibrator.getMeasurementType());
11118 assertFalse(calibrator.isOrderedMeasurementsRequired());
11119 assertFalse(calibrator.isQualityScoresRequired());
11120 assertFalse(calibrator.isCommonAxisUsed());
11121 assertNull(calibrator.getListener());
11122 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
11123 assertFalse(calibrator.isReady());
11124 assertFalse(calibrator.isRunning());
11125 assertNull(calibrator.getEstimatedMa());
11126 assertNull(calibrator.getEstimatedSx());
11127 assertNull(calibrator.getEstimatedSy());
11128 assertNull(calibrator.getEstimatedSz());
11129 assertNull(calibrator.getEstimatedMxy());
11130 assertNull(calibrator.getEstimatedMxz());
11131 assertNull(calibrator.getEstimatedMyx());
11132 assertNull(calibrator.getEstimatedMyz());
11133 assertNull(calibrator.getEstimatedMzx());
11134 assertNull(calibrator.getEstimatedMzy());
11135 assertNull(calibrator.getEstimatedCovariance());
11136 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11137 assertNotNull(calibrator.getGroundTruthGravityNorm());
11138 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11139 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11140 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11141 ABSOLUTE_ERROR));
11142 final var g = new Acceleration(0.0, AccelerationUnit.G);
11143 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11144 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11145 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11146
11147
11148 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11149 -gravityNorm, biasX, biasY, biasZ, sx, sy, sz));
11150 }
11151
11152 @Test
11153 void testConstructor104() throws WrongSizeException {
11154 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
11155
11156 final var ba = generateBa();
11157 final var biasX = ba.getElementAtIndex(0);
11158 final var biasY = ba.getElementAtIndex(1);
11159 final var biasZ = ba.getElementAtIndex(2);
11160
11161 final var ma = generateMaCommonAxis();
11162 final var sx = ma.getElementAt(0, 0);
11163 final var sy = ma.getElementAt(1, 1);
11164 final var sz = ma.getElementAt(2, 2);
11165
11166 final var randomizer = new UniformRandomizer();
11167 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11168 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11169 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11170 final var nedPosition = new NEDPosition(latitude, longitude, height);
11171 final var nedVelocity = new NEDVelocity();
11172 final var ecefPosition = new ECEFPosition();
11173 final var ecefVelocity = new ECEFVelocity();
11174 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11175 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11176 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11177 final var gravityNorm = gravity.getNorm();
11178
11179 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
11180 biasX, biasY, biasZ, sx, sy, sz);
11181
11182
11183 assertEquals(biasX, calibrator.getBiasX(), 0.0);
11184 assertEquals(biasY, calibrator.getBiasY(), 0.0);
11185 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11186 final var bx1 = calibrator.getBiasXAsAcceleration();
11187 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11188 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11189 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11190 calibrator.getBiasXAsAcceleration(bx2);
11191 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11192 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11193 final var by1 = calibrator.getBiasYAsAcceleration();
11194 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11195 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11196 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11197 calibrator.getBiasYAsAcceleration(by2);
11198 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11199 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11200 final var bz1 = calibrator.getBiasZAsAcceleration();
11201 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11202 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11203 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11204 calibrator.getBiasZAsAcceleration(bz2);
11205 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11206 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11207 final var biasTriad1 = calibrator.getBiasAsTriad();
11208 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11209 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11210 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11211 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11212 final var biasTriad2 = new AccelerationTriad();
11213 calibrator.getBiasAsTriad(biasTriad2);
11214 assertEquals(biasTriad1, biasTriad2);
11215 assertEquals(sx, calibrator.getInitialSx(), 0.0);
11216 assertEquals(sy, calibrator.getInitialSy(), 0.0);
11217 assertEquals(sz, calibrator.getInitialSz(), 0.0);
11218 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11219 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11220 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11221 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11222 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11223 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11224 final var bias1 = calibrator.getBias();
11225 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11226 final var bias2 = new double[3];
11227 calibrator.getBias(bias2);
11228 assertArrayEquals(bias1, bias2, 0.0);
11229 final var b1 = calibrator.getBiasAsMatrix();
11230 assertEquals(b1, ba);
11231 final var b2 = new Matrix(3, 1);
11232 calibrator.getBiasAsMatrix(b2);
11233 assertEquals(b1, b2);
11234 final var ma1 = calibrator.getInitialMa();
11235 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11236 final var ma2 = new Matrix(3, 3);
11237 calibrator.getInitialMa(ma2);
11238 assertEquals(ma1, ma2);
11239 assertSame(measurements, calibrator.getMeasurements());
11240 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11241 calibrator.getMeasurementType());
11242 assertFalse(calibrator.isOrderedMeasurementsRequired());
11243 assertFalse(calibrator.isQualityScoresRequired());
11244 assertFalse(calibrator.isCommonAxisUsed());
11245 assertNull(calibrator.getListener());
11246 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
11247 assertFalse(calibrator.isReady());
11248 assertFalse(calibrator.isRunning());
11249 assertNull(calibrator.getEstimatedMa());
11250 assertNull(calibrator.getEstimatedSx());
11251 assertNull(calibrator.getEstimatedSy());
11252 assertNull(calibrator.getEstimatedSz());
11253 assertNull(calibrator.getEstimatedMxy());
11254 assertNull(calibrator.getEstimatedMxz());
11255 assertNull(calibrator.getEstimatedMyx());
11256 assertNull(calibrator.getEstimatedMyz());
11257 assertNull(calibrator.getEstimatedMzx());
11258 assertNull(calibrator.getEstimatedMzy());
11259 assertNull(calibrator.getEstimatedCovariance());
11260 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11261 assertNotNull(calibrator.getGroundTruthGravityNorm());
11262 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11263 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11264 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11265 ABSOLUTE_ERROR));
11266 final var g = new Acceleration(0.0, AccelerationUnit.G);
11267 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11268 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11269 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11270
11271
11272 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11273 -gravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz));
11274 }
11275
11276 @Test
11277 void testConstructor105() throws WrongSizeException {
11278 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
11279
11280 final var ba = generateBa();
11281 final var biasX = ba.getElementAtIndex(0);
11282 final var biasY = ba.getElementAtIndex(1);
11283 final var biasZ = ba.getElementAtIndex(2);
11284
11285 final var ma = generateMaCommonAxis();
11286 final var sx = ma.getElementAt(0, 0);
11287 final var sy = ma.getElementAt(1, 1);
11288 final var sz = ma.getElementAt(2, 2);
11289
11290 final var randomizer = new UniformRandomizer();
11291 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11292 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11293 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11294 final var nedPosition = new NEDPosition(latitude, longitude, height);
11295 final var nedVelocity = new NEDVelocity();
11296 final var ecefPosition = new ECEFPosition();
11297 final var ecefVelocity = new ECEFVelocity();
11298 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11299 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11300 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11301 final var gravityNorm = gravity.getNorm();
11302
11303 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
11304 biasX, biasY, biasZ, sx, sy, sz, this);
11305
11306
11307 assertEquals(biasX, calibrator.getBiasX(), 0.0);
11308 assertEquals(biasY, calibrator.getBiasY(), 0.0);
11309 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11310 final var bx1 = calibrator.getBiasXAsAcceleration();
11311 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11312 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11313 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11314 calibrator.getBiasXAsAcceleration(bx2);
11315 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11316 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11317 final var by1 = calibrator.getBiasYAsAcceleration();
11318 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11319 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11320 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11321 calibrator.getBiasYAsAcceleration(by2);
11322 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11323 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11324 final var bz1 = calibrator.getBiasZAsAcceleration();
11325 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11326 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11327 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11328 calibrator.getBiasZAsAcceleration(bz2);
11329 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11330 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11331 final var biasTriad1 = calibrator.getBiasAsTriad();
11332 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11333 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11334 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11335 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11336 final var biasTriad2 = new AccelerationTriad();
11337 calibrator.getBiasAsTriad(biasTriad2);
11338 assertEquals(biasTriad1, biasTriad2);
11339 assertEquals(sx, calibrator.getInitialSx(), 0.0);
11340 assertEquals(sy, calibrator.getInitialSy(), 0.0);
11341 assertEquals(sz, calibrator.getInitialSz(), 0.0);
11342 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11343 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11344 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11345 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11346 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11347 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11348 final var bias1 = calibrator.getBias();
11349 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11350 final var bias2 = new double[3];
11351 calibrator.getBias(bias2);
11352 assertArrayEquals(bias1, bias2, 0.0);
11353 final var b1 = calibrator.getBiasAsMatrix();
11354 assertEquals(b1, ba);
11355 final var b2 = new Matrix(3, 1);
11356 calibrator.getBiasAsMatrix(b2);
11357 assertEquals(b1, b2);
11358 final var ma1 = calibrator.getInitialMa();
11359 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11360 final var ma2 = new Matrix(3, 3);
11361 calibrator.getInitialMa(ma2);
11362 assertEquals(ma1, ma2);
11363 assertSame(measurements, calibrator.getMeasurements());
11364 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11365 calibrator.getMeasurementType());
11366 assertFalse(calibrator.isOrderedMeasurementsRequired());
11367 assertFalse(calibrator.isQualityScoresRequired());
11368 assertFalse(calibrator.isCommonAxisUsed());
11369 assertSame(this, calibrator.getListener());
11370 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
11371 assertFalse(calibrator.isReady());
11372 assertFalse(calibrator.isRunning());
11373 assertNull(calibrator.getEstimatedMa());
11374 assertNull(calibrator.getEstimatedSx());
11375 assertNull(calibrator.getEstimatedSy());
11376 assertNull(calibrator.getEstimatedSz());
11377 assertNull(calibrator.getEstimatedMxy());
11378 assertNull(calibrator.getEstimatedMxz());
11379 assertNull(calibrator.getEstimatedMyx());
11380 assertNull(calibrator.getEstimatedMyz());
11381 assertNull(calibrator.getEstimatedMzx());
11382 assertNull(calibrator.getEstimatedMzy());
11383 assertNull(calibrator.getEstimatedCovariance());
11384 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11385 assertNotNull(calibrator.getGroundTruthGravityNorm());
11386 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11387 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11388 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11389 ABSOLUTE_ERROR));
11390 final var g = new Acceleration(0.0, AccelerationUnit.G);
11391 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11392 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11393 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11394
11395
11396 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11397 -gravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz, this));
11398 }
11399
11400 @Test
11401 void testConstructor106() throws WrongSizeException {
11402 final var ba = generateBa();
11403 final var biasX = ba.getElementAtIndex(0);
11404 final var biasY = ba.getElementAtIndex(1);
11405 final var biasZ = ba.getElementAtIndex(2);
11406
11407 final var ma = generateMaCommonAxis();
11408 final var sx = ma.getElementAt(0, 0);
11409 final var sy = ma.getElementAt(1, 1);
11410 final var sz = ma.getElementAt(2, 2);
11411
11412 final var randomizer = new UniformRandomizer();
11413 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11414 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11415 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11416 final var nedPosition = new NEDPosition(latitude, longitude, height);
11417 final var nedVelocity = new NEDVelocity();
11418 final var ecefPosition = new ECEFPosition();
11419 final var ecefVelocity = new ECEFVelocity();
11420 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11421 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11422 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11423 final var gravityNorm = gravity.getNorm();
11424
11425 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
11426 biasX, biasY, biasZ, sx, sy, sz);
11427
11428
11429 assertEquals(biasX, calibrator.getBiasX(), 0.0);
11430 assertEquals(biasY, calibrator.getBiasY(), 0.0);
11431 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11432 final var bx1 = calibrator.getBiasXAsAcceleration();
11433 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11434 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11435 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11436 calibrator.getBiasXAsAcceleration(bx2);
11437 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11438 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11439 final var by1 = calibrator.getBiasYAsAcceleration();
11440 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11441 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11442 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11443 calibrator.getBiasYAsAcceleration(by2);
11444 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11445 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11446 final var bz1 = calibrator.getBiasZAsAcceleration();
11447 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11448 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11449 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11450 calibrator.getBiasZAsAcceleration(bz2);
11451 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11452 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11453 final var biasTriad1 = calibrator.getBiasAsTriad();
11454 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11455 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11456 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11457 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11458 final var biasTriad2 = new AccelerationTriad();
11459 calibrator.getBiasAsTriad(biasTriad2);
11460 assertEquals(biasTriad1, biasTriad2);
11461 assertEquals(sx, calibrator.getInitialSx(), 0.0);
11462 assertEquals(sy, calibrator.getInitialSy(), 0.0);
11463 assertEquals(sz, calibrator.getInitialSz(), 0.0);
11464 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11465 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11466 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11467 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11468 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11469 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11470 final var bias1 = calibrator.getBias();
11471 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11472 final var bias2 = new double[3];
11473 calibrator.getBias(bias2);
11474 assertArrayEquals(bias1, bias2, 0.0);
11475 final var b1 = calibrator.getBiasAsMatrix();
11476 assertEquals(b1, ba);
11477 final var b2 = new Matrix(3, 1);
11478 calibrator.getBiasAsMatrix(b2);
11479 assertEquals(b1, b2);
11480 final var ma1 = calibrator.getInitialMa();
11481 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11482 final var ma2 = new Matrix(3, 3);
11483 calibrator.getInitialMa(ma2);
11484 assertEquals(ma1, ma2);
11485 assertNull(calibrator.getMeasurements());
11486 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11487 calibrator.getMeasurementType());
11488 assertFalse(calibrator.isOrderedMeasurementsRequired());
11489 assertFalse(calibrator.isQualityScoresRequired());
11490 assertTrue(calibrator.isCommonAxisUsed());
11491 assertNull(calibrator.getListener());
11492 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
11493 assertFalse(calibrator.isReady());
11494 assertFalse(calibrator.isRunning());
11495 assertNull(calibrator.getEstimatedMa());
11496 assertNull(calibrator.getEstimatedSx());
11497 assertNull(calibrator.getEstimatedSy());
11498 assertNull(calibrator.getEstimatedSz());
11499 assertNull(calibrator.getEstimatedMxy());
11500 assertNull(calibrator.getEstimatedMxz());
11501 assertNull(calibrator.getEstimatedMyx());
11502 assertNull(calibrator.getEstimatedMyz());
11503 assertNull(calibrator.getEstimatedMzx());
11504 assertNull(calibrator.getEstimatedMzy());
11505 assertNull(calibrator.getEstimatedCovariance());
11506 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11507 assertNotNull(calibrator.getGroundTruthGravityNorm());
11508 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11509 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11510 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11511 ABSOLUTE_ERROR));
11512 final var g = new Acceleration(0.0, AccelerationUnit.G);
11513 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11514 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11515 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11516
11517
11518 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11519 -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz));
11520 }
11521
11522 @Test
11523 void testConstructor107() throws WrongSizeException {
11524 final var ba = generateBa();
11525 final var biasX = ba.getElementAtIndex(0);
11526 final var biasY = ba.getElementAtIndex(1);
11527 final var biasZ = ba.getElementAtIndex(2);
11528
11529 final var ma = generateMaCommonAxis();
11530 final var sx = ma.getElementAt(0, 0);
11531 final var sy = ma.getElementAt(1, 1);
11532 final var sz = ma.getElementAt(2, 2);
11533
11534 final var randomizer = new UniformRandomizer();
11535 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11536 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11537 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11538 final var nedPosition = new NEDPosition(latitude, longitude, height);
11539 final var nedVelocity = new NEDVelocity();
11540 final var ecefPosition = new ECEFPosition();
11541 final var ecefVelocity = new ECEFVelocity();
11542 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11543 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11544 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11545 final var gravityNorm = gravity.getNorm();
11546
11547 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
11548 biasX, biasY, biasZ, sx, sy, sz, this);
11549
11550
11551 assertEquals(biasX, calibrator.getBiasX(), 0.0);
11552 assertEquals(biasY, calibrator.getBiasY(), 0.0);
11553 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11554 final var bx1 = calibrator.getBiasXAsAcceleration();
11555 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11556 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11557 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11558 calibrator.getBiasXAsAcceleration(bx2);
11559 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11560 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11561 final var by1 = calibrator.getBiasYAsAcceleration();
11562 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11563 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11564 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11565 calibrator.getBiasYAsAcceleration(by2);
11566 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11567 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11568 final var bz1 = calibrator.getBiasZAsAcceleration();
11569 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11570 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11571 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11572 calibrator.getBiasZAsAcceleration(bz2);
11573 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11574 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11575 final var biasTriad1 = calibrator.getBiasAsTriad();
11576 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11577 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11578 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11579 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11580 final var biasTriad2 = new AccelerationTriad();
11581 calibrator.getBiasAsTriad(biasTriad2);
11582 assertEquals(biasTriad1, biasTriad2);
11583 assertEquals(sx, calibrator.getInitialSx(), 0.0);
11584 assertEquals(sy, calibrator.getInitialSy(), 0.0);
11585 assertEquals(sz, calibrator.getInitialSz(), 0.0);
11586 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11587 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11588 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11589 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11590 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11591 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11592 final var bias1 = calibrator.getBias();
11593 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11594 final var bias2 = new double[3];
11595 calibrator.getBias(bias2);
11596 assertArrayEquals(bias1, bias2, 0.0);
11597 final var b1 = calibrator.getBiasAsMatrix();
11598 assertEquals(b1, ba);
11599 final var b2 = new Matrix(3, 1);
11600 calibrator.getBiasAsMatrix(b2);
11601 assertEquals(b1, b2);
11602 final var ma1 = calibrator.getInitialMa();
11603 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11604 final var ma2 = new Matrix(3, 3);
11605 calibrator.getInitialMa(ma2);
11606 assertEquals(ma1, ma2);
11607 assertNull(calibrator.getMeasurements());
11608 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11609 calibrator.getMeasurementType());
11610 assertFalse(calibrator.isOrderedMeasurementsRequired());
11611 assertFalse(calibrator.isQualityScoresRequired());
11612 assertTrue(calibrator.isCommonAxisUsed());
11613 assertSame(this, calibrator.getListener());
11614 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
11615 assertFalse(calibrator.isReady());
11616 assertFalse(calibrator.isRunning());
11617 assertNull(calibrator.getEstimatedMa());
11618 assertNull(calibrator.getEstimatedSx());
11619 assertNull(calibrator.getEstimatedSy());
11620 assertNull(calibrator.getEstimatedSz());
11621 assertNull(calibrator.getEstimatedMxy());
11622 assertNull(calibrator.getEstimatedMxz());
11623 assertNull(calibrator.getEstimatedMyx());
11624 assertNull(calibrator.getEstimatedMyz());
11625 assertNull(calibrator.getEstimatedMzx());
11626 assertNull(calibrator.getEstimatedMzy());
11627 assertNull(calibrator.getEstimatedCovariance());
11628 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11629 assertNotNull(calibrator.getGroundTruthGravityNorm());
11630 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11631 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11632 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11633 ABSOLUTE_ERROR));
11634 final var g = new Acceleration(0.0, AccelerationUnit.G);
11635 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11636 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11637 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11638
11639
11640 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11641 -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz, this));
11642 }
11643
11644 @Test
11645 void testConstructor108() throws WrongSizeException {
11646 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
11647
11648 final var ba = generateBa();
11649 final var biasX = ba.getElementAtIndex(0);
11650 final var biasY = ba.getElementAtIndex(1);
11651 final var biasZ = ba.getElementAtIndex(2);
11652
11653 final var ma = generateMaCommonAxis();
11654 final var sx = ma.getElementAt(0, 0);
11655 final var sy = ma.getElementAt(1, 1);
11656 final var sz = ma.getElementAt(2, 2);
11657
11658 final var randomizer = new UniformRandomizer();
11659 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11660 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11661 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11662 final var nedPosition = new NEDPosition(latitude, longitude, height);
11663 final var nedVelocity = new NEDVelocity();
11664 final var ecefPosition = new ECEFPosition();
11665 final var ecefVelocity = new ECEFVelocity();
11666 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11667 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11668 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11669 final var gravityNorm = gravity.getNorm();
11670
11671 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
11672 true, biasX, biasY, biasZ, sx, sy, sz);
11673
11674
11675 assertEquals(biasX, calibrator.getBiasX(), 0.0);
11676 assertEquals(biasY, calibrator.getBiasY(), 0.0);
11677 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11678 final var bx1 = calibrator.getBiasXAsAcceleration();
11679 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11680 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11681 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11682 calibrator.getBiasXAsAcceleration(bx2);
11683 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11684 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11685 final var by1 = calibrator.getBiasYAsAcceleration();
11686 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11687 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11688 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11689 calibrator.getBiasYAsAcceleration(by2);
11690 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11691 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11692 final var bz1 = calibrator.getBiasZAsAcceleration();
11693 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11694 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11695 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11696 calibrator.getBiasZAsAcceleration(bz2);
11697 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11698 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11699 final var biasTriad1 = calibrator.getBiasAsTriad();
11700 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11701 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11702 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11703 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11704 final var biasTriad2 = new AccelerationTriad();
11705 calibrator.getBiasAsTriad(biasTriad2);
11706 assertEquals(biasTriad1, biasTriad2);
11707 assertEquals(sx, calibrator.getInitialSx(), 0.0);
11708 assertEquals(sy, calibrator.getInitialSy(), 0.0);
11709 assertEquals(sz, calibrator.getInitialSz(), 0.0);
11710 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11711 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11712 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11713 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11714 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11715 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11716 final var bias1 = calibrator.getBias();
11717 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11718 final var bias2 = new double[3];
11719 calibrator.getBias(bias2);
11720 assertArrayEquals(bias1, bias2, 0.0);
11721 final var b1 = calibrator.getBiasAsMatrix();
11722 assertEquals(b1, ba);
11723 final var b2 = new Matrix(3, 1);
11724 calibrator.getBiasAsMatrix(b2);
11725 assertEquals(b1, b2);
11726 final var ma1 = calibrator.getInitialMa();
11727 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11728 final var ma2 = new Matrix(3, 3);
11729 calibrator.getInitialMa(ma2);
11730 assertEquals(ma1, ma2);
11731 assertSame(measurements, calibrator.getMeasurements());
11732 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11733 calibrator.getMeasurementType());
11734 assertFalse(calibrator.isOrderedMeasurementsRequired());
11735 assertFalse(calibrator.isQualityScoresRequired());
11736 assertTrue(calibrator.isCommonAxisUsed());
11737 assertNull(calibrator.getListener());
11738 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
11739 assertFalse(calibrator.isReady());
11740 assertFalse(calibrator.isRunning());
11741 assertNull(calibrator.getEstimatedMa());
11742 assertNull(calibrator.getEstimatedSx());
11743 assertNull(calibrator.getEstimatedSy());
11744 assertNull(calibrator.getEstimatedSz());
11745 assertNull(calibrator.getEstimatedMxy());
11746 assertNull(calibrator.getEstimatedMxz());
11747 assertNull(calibrator.getEstimatedMyx());
11748 assertNull(calibrator.getEstimatedMyz());
11749 assertNull(calibrator.getEstimatedMzx());
11750 assertNull(calibrator.getEstimatedMzy());
11751 assertNull(calibrator.getEstimatedCovariance());
11752 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11753 assertNotNull(calibrator.getGroundTruthGravityNorm());
11754 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11755 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11756 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11757 ABSOLUTE_ERROR));
11758 final var g = new Acceleration(0.0, AccelerationUnit.G);
11759 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11760 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11761 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11762
11763
11764 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11765 -gravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz));
11766 }
11767
11768 @Test
11769 void testConstructor109() throws WrongSizeException {
11770 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
11771
11772 final var ba = generateBa();
11773 final var biasX = ba.getElementAtIndex(0);
11774 final var biasY = ba.getElementAtIndex(1);
11775 final var biasZ = ba.getElementAtIndex(2);
11776
11777 final var ma = generateMaCommonAxis();
11778 final var sx = ma.getElementAt(0, 0);
11779 final var sy = ma.getElementAt(1, 1);
11780 final var sz = ma.getElementAt(2, 2);
11781
11782 final var randomizer = new UniformRandomizer();
11783 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11784 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11785 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11786 final var nedPosition = new NEDPosition(latitude, longitude, height);
11787 final var nedVelocity = new NEDVelocity();
11788 final var ecefPosition = new ECEFPosition();
11789 final var ecefVelocity = new ECEFVelocity();
11790 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11791 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11792 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11793 final var gravityNorm = gravity.getNorm();
11794
11795 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
11796 true, biasX, biasY, biasZ, sx, sy, sz, this);
11797
11798
11799 assertEquals(biasX, calibrator.getBiasX(), 0.0);
11800 assertEquals(biasY, calibrator.getBiasY(), 0.0);
11801 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11802 final var bx1 = calibrator.getBiasXAsAcceleration();
11803 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11804 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11805 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11806 calibrator.getBiasXAsAcceleration(bx2);
11807 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11808 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11809 final var by1 = calibrator.getBiasYAsAcceleration();
11810 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11811 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11812 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11813 calibrator.getBiasYAsAcceleration(by2);
11814 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11815 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11816 final var bz1 = calibrator.getBiasZAsAcceleration();
11817 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11818 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11819 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11820 calibrator.getBiasZAsAcceleration(bz2);
11821 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11822 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11823 final var biasTriad1 = calibrator.getBiasAsTriad();
11824 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11825 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11826 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11827 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11828 final var biasTriad2 = new AccelerationTriad();
11829 calibrator.getBiasAsTriad(biasTriad2);
11830 assertEquals(biasTriad1, biasTriad2);
11831 assertEquals(sx, calibrator.getInitialSx(), 0.0);
11832 assertEquals(sy, calibrator.getInitialSy(), 0.0);
11833 assertEquals(sz, calibrator.getInitialSz(), 0.0);
11834 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11835 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11836 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11837 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11838 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11839 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11840 final var bias1 = calibrator.getBias();
11841 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11842 final var bias2 = new double[3];
11843 calibrator.getBias(bias2);
11844 assertArrayEquals(bias1, bias2, 0.0);
11845 final var b1 = calibrator.getBiasAsMatrix();
11846 assertEquals(b1, ba);
11847 final var b2 = new Matrix(3, 1);
11848 calibrator.getBiasAsMatrix(b2);
11849 assertEquals(b1, b2);
11850 final var ma1 = calibrator.getInitialMa();
11851 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11852 final var ma2 = new Matrix(3, 3);
11853 calibrator.getInitialMa(ma2);
11854 assertEquals(ma1, ma2);
11855 assertSame(measurements, calibrator.getMeasurements());
11856 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11857 calibrator.getMeasurementType());
11858 assertFalse(calibrator.isOrderedMeasurementsRequired());
11859 assertFalse(calibrator.isQualityScoresRequired());
11860 assertTrue(calibrator.isCommonAxisUsed());
11861 assertSame(this, calibrator.getListener());
11862 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
11863 assertFalse(calibrator.isReady());
11864 assertFalse(calibrator.isRunning());
11865 assertNull(calibrator.getEstimatedMa());
11866 assertNull(calibrator.getEstimatedSx());
11867 assertNull(calibrator.getEstimatedSy());
11868 assertNull(calibrator.getEstimatedSz());
11869 assertNull(calibrator.getEstimatedMxy());
11870 assertNull(calibrator.getEstimatedMxz());
11871 assertNull(calibrator.getEstimatedMyx());
11872 assertNull(calibrator.getEstimatedMyz());
11873 assertNull(calibrator.getEstimatedMzx());
11874 assertNull(calibrator.getEstimatedMzy());
11875 assertNull(calibrator.getEstimatedCovariance());
11876 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11877 assertNotNull(calibrator.getGroundTruthGravityNorm());
11878 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11879 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11880 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11881 ABSOLUTE_ERROR));
11882 final var g = new Acceleration(0.0, AccelerationUnit.G);
11883 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11884 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11885 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11886
11887
11888 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11889 -gravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz, this));
11890 }
11891
11892 @Test
11893 void testConstructor110() throws WrongSizeException {
11894 final var ba = generateBa();
11895 final var biasX = ba.getElementAtIndex(0);
11896 final var biasY = ba.getElementAtIndex(1);
11897 final var biasZ = ba.getElementAtIndex(2);
11898
11899 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
11900 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
11901 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
11902
11903 final var ma = generateMaCommonAxis();
11904 final var sx = ma.getElementAt(0, 0);
11905 final var sy = ma.getElementAt(1, 1);
11906 final var sz = ma.getElementAt(2, 2);
11907
11908 final var randomizer = new UniformRandomizer();
11909 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11910 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11911 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11912 final var nedPosition = new NEDPosition(latitude, longitude, height);
11913 final var nedVelocity = new NEDVelocity();
11914 final var ecefPosition = new ECEFPosition();
11915 final var ecefVelocity = new ECEFVelocity();
11916 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11917 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11918 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11919 final var gravityNorm = gravity.getNorm();
11920
11921 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz);
11922
11923
11924 assertEquals(biasX, calibrator.getBiasX(), 0.0);
11925 assertEquals(biasY, calibrator.getBiasY(), 0.0);
11926 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11927 final var bx1 = calibrator.getBiasXAsAcceleration();
11928 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11929 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11930 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11931 calibrator.getBiasXAsAcceleration(bx2);
11932 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11933 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11934 final var by1 = calibrator.getBiasYAsAcceleration();
11935 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11936 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11937 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11938 calibrator.getBiasYAsAcceleration(by2);
11939 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11940 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11941 final var bz1 = calibrator.getBiasZAsAcceleration();
11942 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11943 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11944 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11945 calibrator.getBiasZAsAcceleration(bz2);
11946 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11947 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11948 final var biasTriad1 = calibrator.getBiasAsTriad();
11949 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11950 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11951 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11952 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11953 final var biasTriad2 = new AccelerationTriad();
11954 calibrator.getBiasAsTriad(biasTriad2);
11955 assertEquals(biasTriad1, biasTriad2);
11956 assertEquals(sx, calibrator.getInitialSx(), 0.0);
11957 assertEquals(sy, calibrator.getInitialSy(), 0.0);
11958 assertEquals(sz, calibrator.getInitialSz(), 0.0);
11959 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11960 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11961 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11962 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11963 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11964 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11965 final var bias1 = calibrator.getBias();
11966 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11967 final var bias2 = new double[3];
11968 calibrator.getBias(bias2);
11969 assertArrayEquals(bias1, bias2, 0.0);
11970 final var b1 = calibrator.getBiasAsMatrix();
11971 assertEquals(b1, ba);
11972 final var b2 = new Matrix(3, 1);
11973 calibrator.getBiasAsMatrix(b2);
11974 assertEquals(b1, b2);
11975 final var ma1 = calibrator.getInitialMa();
11976 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11977 final var ma2 = new Matrix(3, 3);
11978 calibrator.getInitialMa(ma2);
11979 assertEquals(ma1, ma2);
11980 assertNull(calibrator.getMeasurements());
11981 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11982 calibrator.getMeasurementType());
11983 assertFalse(calibrator.isOrderedMeasurementsRequired());
11984 assertFalse(calibrator.isQualityScoresRequired());
11985 assertFalse(calibrator.isCommonAxisUsed());
11986 assertNull(calibrator.getListener());
11987 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
11988 assertFalse(calibrator.isReady());
11989 assertFalse(calibrator.isRunning());
11990 assertNull(calibrator.getEstimatedMa());
11991 assertNull(calibrator.getEstimatedSx());
11992 assertNull(calibrator.getEstimatedSy());
11993 assertNull(calibrator.getEstimatedSz());
11994 assertNull(calibrator.getEstimatedMxy());
11995 assertNull(calibrator.getEstimatedMxz());
11996 assertNull(calibrator.getEstimatedMyx());
11997 assertNull(calibrator.getEstimatedMyz());
11998 assertNull(calibrator.getEstimatedMzx());
11999 assertNull(calibrator.getEstimatedMzy());
12000 assertNull(calibrator.getEstimatedCovariance());
12001 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12002 assertNotNull(calibrator.getGroundTruthGravityNorm());
12003 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12004 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12005 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12006 ABSOLUTE_ERROR));
12007 final var g = new Acceleration(0.0, AccelerationUnit.G);
12008 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12009 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12010 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12011
12012
12013 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12014 -gravityNorm, bx, by, bz, sx, sy, sz));
12015 }
12016
12017 @Test
12018 void testConstructor111() throws WrongSizeException {
12019 final var ba = generateBa();
12020 final var biasX = ba.getElementAtIndex(0);
12021 final var biasY = ba.getElementAtIndex(1);
12022 final var biasZ = ba.getElementAtIndex(2);
12023
12024 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12025 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12026 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12027
12028 final var ma = generateMaCommonAxis();
12029 final var sx = ma.getElementAt(0, 0);
12030 final var sy = ma.getElementAt(1, 1);
12031 final var sz = ma.getElementAt(2, 2);
12032
12033 final var randomizer = new UniformRandomizer();
12034 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12035 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12036 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12037 final var nedPosition = new NEDPosition(latitude, longitude, height);
12038 final var nedVelocity = new NEDVelocity();
12039 final var ecefPosition = new ECEFPosition();
12040 final var ecefVelocity = new ECEFVelocity();
12041 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12042 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12043 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12044 final var gravityNorm = gravity.getNorm();
12045
12046 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz,
12047 this);
12048
12049
12050 assertEquals(biasX, calibrator.getBiasX(), 0.0);
12051 assertEquals(biasY, calibrator.getBiasY(), 0.0);
12052 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12053 final var bx1 = calibrator.getBiasXAsAcceleration();
12054 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12055 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12056 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12057 calibrator.getBiasXAsAcceleration(bx2);
12058 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12059 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12060 final var by1 = calibrator.getBiasYAsAcceleration();
12061 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12062 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12063 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12064 calibrator.getBiasYAsAcceleration(by2);
12065 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12066 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12067 final var bz1 = calibrator.getBiasZAsAcceleration();
12068 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12069 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12070 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12071 calibrator.getBiasZAsAcceleration(bz2);
12072 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12073 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12074 final var biasTriad1 = calibrator.getBiasAsTriad();
12075 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12076 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12077 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12078 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12079 final var biasTriad2 = new AccelerationTriad();
12080 calibrator.getBiasAsTriad(biasTriad2);
12081 assertEquals(biasTriad1, biasTriad2);
12082 assertEquals(sx, calibrator.getInitialSx(), 0.0);
12083 assertEquals(sy, calibrator.getInitialSy(), 0.0);
12084 assertEquals(sz, calibrator.getInitialSz(), 0.0);
12085 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
12086 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
12087 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
12088 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
12089 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
12090 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
12091 final var bias1 = calibrator.getBias();
12092 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12093 final var bias2 = new double[3];
12094 calibrator.getBias(bias2);
12095 assertArrayEquals(bias1, bias2, 0.0);
12096 final var b1 = calibrator.getBiasAsMatrix();
12097 assertEquals(b1, ba);
12098 final var b2 = new Matrix(3, 1);
12099 calibrator.getBiasAsMatrix(b2);
12100 assertEquals(b1, b2);
12101 final var ma1 = calibrator.getInitialMa();
12102 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12103 final var ma2 = new Matrix(3, 3);
12104 calibrator.getInitialMa(ma2);
12105 assertEquals(ma1, ma2);
12106 assertNull(calibrator.getMeasurements());
12107 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
12108 calibrator.getMeasurementType());
12109 assertFalse(calibrator.isOrderedMeasurementsRequired());
12110 assertFalse(calibrator.isQualityScoresRequired());
12111 assertFalse(calibrator.isCommonAxisUsed());
12112 assertSame(this, calibrator.getListener());
12113 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
12114 assertFalse(calibrator.isReady());
12115 assertFalse(calibrator.isRunning());
12116 assertNull(calibrator.getEstimatedMa());
12117 assertNull(calibrator.getEstimatedSx());
12118 assertNull(calibrator.getEstimatedSy());
12119 assertNull(calibrator.getEstimatedSz());
12120 assertNull(calibrator.getEstimatedMxy());
12121 assertNull(calibrator.getEstimatedMxz());
12122 assertNull(calibrator.getEstimatedMyx());
12123 assertNull(calibrator.getEstimatedMyz());
12124 assertNull(calibrator.getEstimatedMzx());
12125 assertNull(calibrator.getEstimatedMzy());
12126 assertNull(calibrator.getEstimatedCovariance());
12127 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12128 assertNotNull(calibrator.getGroundTruthGravityNorm());
12129 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12130 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12131 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12132 ABSOLUTE_ERROR));
12133 final var g = new Acceleration(0.0, AccelerationUnit.G);
12134 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12135 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12136 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12137
12138
12139 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12140 -gravityNorm, bx, by, bz, sx, sy, sz, this));
12141 }
12142
12143 @Test
12144 void testConstructor112() throws WrongSizeException {
12145 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
12146
12147 final var ba = generateBa();
12148 final var biasX = ba.getElementAtIndex(0);
12149 final var biasY = ba.getElementAtIndex(1);
12150 final var biasZ = ba.getElementAtIndex(2);
12151
12152 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12153 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12154 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12155
12156 final var ma = generateMaCommonAxis();
12157 final var sx = ma.getElementAt(0, 0);
12158 final var sy = ma.getElementAt(1, 1);
12159 final var sz = ma.getElementAt(2, 2);
12160
12161 final var randomizer = new UniformRandomizer();
12162 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12163 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12164 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12165 final var nedPosition = new NEDPosition(latitude, longitude, height);
12166 final var nedVelocity = new NEDVelocity();
12167 final var ecefPosition = new ECEFPosition();
12168 final var ecefVelocity = new ECEFVelocity();
12169 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12170 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12171 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12172 final var gravityNorm = gravity.getNorm();
12173
12174 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
12175 sx, sy, sz);
12176
12177
12178 assertEquals(biasX, calibrator.getBiasX(), 0.0);
12179 assertEquals(biasY, calibrator.getBiasY(), 0.0);
12180 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12181 final var bx1 = calibrator.getBiasXAsAcceleration();
12182 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12183 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12184 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12185 calibrator.getBiasXAsAcceleration(bx2);
12186 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12187 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12188 final var by1 = calibrator.getBiasYAsAcceleration();
12189 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12190 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12191 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12192 calibrator.getBiasYAsAcceleration(by2);
12193 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12194 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12195 final var bz1 = calibrator.getBiasZAsAcceleration();
12196 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12197 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12198 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12199 calibrator.getBiasZAsAcceleration(bz2);
12200 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12201 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12202 final var biasTriad1 = calibrator.getBiasAsTriad();
12203 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12204 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12205 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12206 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12207 final var biasTriad2 = new AccelerationTriad();
12208 calibrator.getBiasAsTriad(biasTriad2);
12209 assertEquals(biasTriad1, biasTriad2);
12210 assertEquals(sx, calibrator.getInitialSx(), 0.0);
12211 assertEquals(sy, calibrator.getInitialSy(), 0.0);
12212 assertEquals(sz, calibrator.getInitialSz(), 0.0);
12213 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
12214 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
12215 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
12216 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
12217 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
12218 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
12219 final var bias1 = calibrator.getBias();
12220 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12221 final var bias2 = new double[3];
12222 calibrator.getBias(bias2);
12223 assertArrayEquals(bias1, bias2, 0.0);
12224 final var b1 = calibrator.getBiasAsMatrix();
12225 assertEquals(b1, ba);
12226 final var b2 = new Matrix(3, 1);
12227 calibrator.getBiasAsMatrix(b2);
12228 assertEquals(b1, b2);
12229 final var ma1 = calibrator.getInitialMa();
12230 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12231 final var ma2 = new Matrix(3, 3);
12232 calibrator.getInitialMa(ma2);
12233 assertEquals(ma1, ma2);
12234 assertSame(measurements, calibrator.getMeasurements());
12235 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
12236 calibrator.getMeasurementType());
12237 assertFalse(calibrator.isOrderedMeasurementsRequired());
12238 assertFalse(calibrator.isQualityScoresRequired());
12239 assertFalse(calibrator.isCommonAxisUsed());
12240 assertNull(calibrator.getListener());
12241 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
12242 assertFalse(calibrator.isReady());
12243 assertFalse(calibrator.isRunning());
12244 assertNull(calibrator.getEstimatedMa());
12245 assertNull(calibrator.getEstimatedSx());
12246 assertNull(calibrator.getEstimatedSy());
12247 assertNull(calibrator.getEstimatedSz());
12248 assertNull(calibrator.getEstimatedMxy());
12249 assertNull(calibrator.getEstimatedMxz());
12250 assertNull(calibrator.getEstimatedMyx());
12251 assertNull(calibrator.getEstimatedMyz());
12252 assertNull(calibrator.getEstimatedMzx());
12253 assertNull(calibrator.getEstimatedMzy());
12254 assertNull(calibrator.getEstimatedCovariance());
12255 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12256 assertNotNull(calibrator.getGroundTruthGravityNorm());
12257 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12258 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12259 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12260 ABSOLUTE_ERROR));
12261 final var g = new Acceleration(0.0, AccelerationUnit.G);
12262 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12263 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12264 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12265
12266
12267 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12268 -gravityNorm, measurements, bx, by, bz, sx, sy, sz));
12269 }
12270
12271 @Test
12272 void testConstructor113() throws WrongSizeException {
12273 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
12274
12275 final var ba = generateBa();
12276 final var biasX = ba.getElementAtIndex(0);
12277 final var biasY = ba.getElementAtIndex(1);
12278 final var biasZ = ba.getElementAtIndex(2);
12279
12280 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12281 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12282 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12283
12284 final var ma = generateMaCommonAxis();
12285 final var sx = ma.getElementAt(0, 0);
12286 final var sy = ma.getElementAt(1, 1);
12287 final var sz = ma.getElementAt(2, 2);
12288
12289 final var randomizer = new UniformRandomizer();
12290 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12291 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12292 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12293 final var nedPosition = new NEDPosition(latitude, longitude, height);
12294 final var nedVelocity = new NEDVelocity();
12295 final var ecefPosition = new ECEFPosition();
12296 final var ecefVelocity = new ECEFVelocity();
12297 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12298 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12299 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12300 final var gravityNorm = gravity.getNorm();
12301
12302 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
12303 sx, sy, sz, this);
12304
12305
12306 assertEquals(biasX, calibrator.getBiasX(), 0.0);
12307 assertEquals(biasY, calibrator.getBiasY(), 0.0);
12308 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12309 final var bx1 = calibrator.getBiasXAsAcceleration();
12310 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12311 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12312 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12313 calibrator.getBiasXAsAcceleration(bx2);
12314 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12315 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12316 final var by1 = calibrator.getBiasYAsAcceleration();
12317 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12318 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12319 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12320 calibrator.getBiasYAsAcceleration(by2);
12321 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12322 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12323 final var bz1 = calibrator.getBiasZAsAcceleration();
12324 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12325 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12326 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12327 calibrator.getBiasZAsAcceleration(bz2);
12328 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12329 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12330 final var biasTriad1 = calibrator.getBiasAsTriad();
12331 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12332 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12333 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12334 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12335 final var biasTriad2 = new AccelerationTriad();
12336 calibrator.getBiasAsTriad(biasTriad2);
12337 assertEquals(biasTriad1, biasTriad2);
12338 assertEquals(sx, calibrator.getInitialSx(), 0.0);
12339 assertEquals(sy, calibrator.getInitialSy(), 0.0);
12340 assertEquals(sz, calibrator.getInitialSz(), 0.0);
12341 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
12342 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
12343 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
12344 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
12345 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
12346 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
12347 final var bias1 = calibrator.getBias();
12348 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12349 final var bias2 = new double[3];
12350 calibrator.getBias(bias2);
12351 assertArrayEquals(bias1, bias2, 0.0);
12352 final var b1 = calibrator.getBiasAsMatrix();
12353 assertEquals(b1, ba);
12354 final var b2 = new Matrix(3, 1);
12355 calibrator.getBiasAsMatrix(b2);
12356 assertEquals(b1, b2);
12357 final var ma1 = calibrator.getInitialMa();
12358 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12359 final var ma2 = new Matrix(3, 3);
12360 calibrator.getInitialMa(ma2);
12361 assertEquals(ma1, ma2);
12362 assertSame(measurements, calibrator.getMeasurements());
12363 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
12364 calibrator.getMeasurementType());
12365 assertFalse(calibrator.isOrderedMeasurementsRequired());
12366 assertFalse(calibrator.isQualityScoresRequired());
12367 assertFalse(calibrator.isCommonAxisUsed());
12368 assertSame(this, calibrator.getListener());
12369 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
12370 assertFalse(calibrator.isReady());
12371 assertFalse(calibrator.isRunning());
12372 assertNull(calibrator.getEstimatedMa());
12373 assertNull(calibrator.getEstimatedSx());
12374 assertNull(calibrator.getEstimatedSy());
12375 assertNull(calibrator.getEstimatedSz());
12376 assertNull(calibrator.getEstimatedMxy());
12377 assertNull(calibrator.getEstimatedMxz());
12378 assertNull(calibrator.getEstimatedMyx());
12379 assertNull(calibrator.getEstimatedMyz());
12380 assertNull(calibrator.getEstimatedMzx());
12381 assertNull(calibrator.getEstimatedMzy());
12382 assertNull(calibrator.getEstimatedCovariance());
12383 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12384 assertNotNull(calibrator.getGroundTruthGravityNorm());
12385 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12386 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12387 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12388 ABSOLUTE_ERROR));
12389 final var g = new Acceleration(0.0, AccelerationUnit.G);
12390 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12391 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12392 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12393
12394
12395 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12396 -gravityNorm, measurements, bx, by, bz, sx, sy, sz, this));
12397 }
12398
12399 @Test
12400 void testConstructor114() throws WrongSizeException {
12401 final var ba = generateBa();
12402 final var biasX = ba.getElementAtIndex(0);
12403 final var biasY = ba.getElementAtIndex(1);
12404 final var biasZ = ba.getElementAtIndex(2);
12405
12406 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12407 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12408 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12409
12410 final var ma = generateMaCommonAxis();
12411 final var sx = ma.getElementAt(0, 0);
12412 final var sy = ma.getElementAt(1, 1);
12413 final var sz = ma.getElementAt(2, 2);
12414
12415 final var randomizer = new UniformRandomizer();
12416 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12417 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12418 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12419 final var nedPosition = new NEDPosition(latitude, longitude, height);
12420 final var nedVelocity = new NEDVelocity();
12421 final var ecefPosition = new ECEFPosition();
12422 final var ecefVelocity = new ECEFVelocity();
12423 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12424 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12425 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12426 final var gravityNorm = gravity.getNorm();
12427
12428 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
12429 bx, by, bz, sx, sy, sz);
12430
12431
12432 assertEquals(biasX, calibrator.getBiasX(), 0.0);
12433 assertEquals(biasY, calibrator.getBiasY(), 0.0);
12434 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12435 final var bx1 = calibrator.getBiasXAsAcceleration();
12436 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12437 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12438 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12439 calibrator.getBiasXAsAcceleration(bx2);
12440 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12441 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12442 final var by1 = calibrator.getBiasYAsAcceleration();
12443 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12444 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12445 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12446 calibrator.getBiasYAsAcceleration(by2);
12447 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12448 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12449 final var bz1 = calibrator.getBiasZAsAcceleration();
12450 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12451 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12452 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12453 calibrator.getBiasZAsAcceleration(bz2);
12454 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12455 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12456 final var biasTriad1 = calibrator.getBiasAsTriad();
12457 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12458 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12459 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12460 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12461 final var biasTriad2 = new AccelerationTriad();
12462 calibrator.getBiasAsTriad(biasTriad2);
12463 assertEquals(biasTriad1, biasTriad2);
12464 assertEquals(sx, calibrator.getInitialSx(), 0.0);
12465 assertEquals(sy, calibrator.getInitialSy(), 0.0);
12466 assertEquals(sz, calibrator.getInitialSz(), 0.0);
12467 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
12468 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
12469 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
12470 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
12471 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
12472 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
12473 final var bias1 = calibrator.getBias();
12474 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12475 final var bias2 = new double[3];
12476 calibrator.getBias(bias2);
12477 assertArrayEquals(bias1, bias2, 0.0);
12478 final var b1 = calibrator.getBiasAsMatrix();
12479 assertEquals(b1, ba);
12480 final var b2 = new Matrix(3, 1);
12481 calibrator.getBiasAsMatrix(b2);
12482 assertEquals(b1, b2);
12483 final var ma1 = calibrator.getInitialMa();
12484 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12485 final var ma2 = new Matrix(3, 3);
12486 calibrator.getInitialMa(ma2);
12487 assertEquals(ma1, ma2);
12488 assertNull(calibrator.getMeasurements());
12489 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
12490 calibrator.getMeasurementType());
12491 assertFalse(calibrator.isOrderedMeasurementsRequired());
12492 assertFalse(calibrator.isQualityScoresRequired());
12493 assertTrue(calibrator.isCommonAxisUsed());
12494 assertNull(calibrator.getListener());
12495 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
12496 assertFalse(calibrator.isReady());
12497 assertFalse(calibrator.isRunning());
12498 assertNull(calibrator.getEstimatedMa());
12499 assertNull(calibrator.getEstimatedSx());
12500 assertNull(calibrator.getEstimatedSy());
12501 assertNull(calibrator.getEstimatedSz());
12502 assertNull(calibrator.getEstimatedMxy());
12503 assertNull(calibrator.getEstimatedMxz());
12504 assertNull(calibrator.getEstimatedMyx());
12505 assertNull(calibrator.getEstimatedMyz());
12506 assertNull(calibrator.getEstimatedMzx());
12507 assertNull(calibrator.getEstimatedMzy());
12508 assertNull(calibrator.getEstimatedCovariance());
12509 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12510 assertNotNull(calibrator.getGroundTruthGravityNorm());
12511 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12512 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12513 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12514 ABSOLUTE_ERROR));
12515 final var g = new Acceleration(0.0, AccelerationUnit.G);
12516 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12517 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12518 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12519
12520
12521 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12522 -gravityNorm, true, bx, by, bz, sx, sy, sz));
12523 }
12524
12525 @Test
12526 void testConstructor115() throws WrongSizeException {
12527 final var ba = generateBa();
12528 final var biasX = ba.getElementAtIndex(0);
12529 final var biasY = ba.getElementAtIndex(1);
12530 final var biasZ = ba.getElementAtIndex(2);
12531
12532 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12533 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12534 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12535
12536 final var ma = generateMaCommonAxis();
12537 final var sx = ma.getElementAt(0, 0);
12538 final var sy = ma.getElementAt(1, 1);
12539 final var sz = ma.getElementAt(2, 2);
12540
12541 final var randomizer = new UniformRandomizer();
12542 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12543 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12544 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12545 final var nedPosition = new NEDPosition(latitude, longitude, height);
12546 final var nedVelocity = new NEDVelocity();
12547 final var ecefPosition = new ECEFPosition();
12548 final var ecefVelocity = new ECEFVelocity();
12549 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12550 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12551 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12552 final var gravityNorm = gravity.getNorm();
12553
12554 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
12555 bx, by, bz, sx, sy, sz, this);
12556
12557
12558 assertEquals(biasX, calibrator.getBiasX(), 0.0);
12559 assertEquals(biasY, calibrator.getBiasY(), 0.0);
12560 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12561 final var bx1 = calibrator.getBiasXAsAcceleration();
12562 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12563 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12564 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12565 calibrator.getBiasXAsAcceleration(bx2);
12566 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12567 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12568 final var by1 = calibrator.getBiasYAsAcceleration();
12569 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12570 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12571 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12572 calibrator.getBiasYAsAcceleration(by2);
12573 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12574 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12575 final var bz1 = calibrator.getBiasZAsAcceleration();
12576 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12577 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12578 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12579 calibrator.getBiasZAsAcceleration(bz2);
12580 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12581 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12582 final var biasTriad1 = calibrator.getBiasAsTriad();
12583 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12584 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12585 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12586 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12587 final var biasTriad2 = new AccelerationTriad();
12588 calibrator.getBiasAsTriad(biasTriad2);
12589 assertEquals(biasTriad1, biasTriad2);
12590 assertEquals(sx, calibrator.getInitialSx(), 0.0);
12591 assertEquals(sy, calibrator.getInitialSy(), 0.0);
12592 assertEquals(sz, calibrator.getInitialSz(), 0.0);
12593 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
12594 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
12595 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
12596 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
12597 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
12598 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
12599 final var bias1 = calibrator.getBias();
12600 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12601 final var bias2 = new double[3];
12602 calibrator.getBias(bias2);
12603 assertArrayEquals(bias1, bias2, 0.0);
12604 final var b1 = calibrator.getBiasAsMatrix();
12605 assertEquals(b1, ba);
12606 final var b2 = new Matrix(3, 1);
12607 calibrator.getBiasAsMatrix(b2);
12608 assertEquals(b1, b2);
12609 final var ma1 = calibrator.getInitialMa();
12610 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12611 final var ma2 = new Matrix(3, 3);
12612 calibrator.getInitialMa(ma2);
12613 assertEquals(ma1, ma2);
12614 assertNull(calibrator.getMeasurements());
12615 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
12616 calibrator.getMeasurementType());
12617 assertFalse(calibrator.isOrderedMeasurementsRequired());
12618 assertFalse(calibrator.isQualityScoresRequired());
12619 assertTrue(calibrator.isCommonAxisUsed());
12620 assertSame(this, calibrator.getListener());
12621 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
12622 assertFalse(calibrator.isReady());
12623 assertFalse(calibrator.isRunning());
12624 assertNull(calibrator.getEstimatedMa());
12625 assertNull(calibrator.getEstimatedSx());
12626 assertNull(calibrator.getEstimatedSy());
12627 assertNull(calibrator.getEstimatedSz());
12628 assertNull(calibrator.getEstimatedMxy());
12629 assertNull(calibrator.getEstimatedMxz());
12630 assertNull(calibrator.getEstimatedMyx());
12631 assertNull(calibrator.getEstimatedMyz());
12632 assertNull(calibrator.getEstimatedMzx());
12633 assertNull(calibrator.getEstimatedMzy());
12634 assertNull(calibrator.getEstimatedCovariance());
12635 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12636 assertNotNull(calibrator.getGroundTruthGravityNorm());
12637 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12638 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12639 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12640 ABSOLUTE_ERROR));
12641 final var g = new Acceleration(0.0, AccelerationUnit.G);
12642 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12643 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12644 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12645
12646
12647 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12648 -gravityNorm, true, bx, by, bz, sx, sy, sz, this));
12649 }
12650
12651 @Test
12652 void testConstructor116() throws WrongSizeException {
12653 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
12654
12655 final var ba = generateBa();
12656 final var biasX = ba.getElementAtIndex(0);
12657 final var biasY = ba.getElementAtIndex(1);
12658 final var biasZ = ba.getElementAtIndex(2);
12659
12660 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12661 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12662 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12663
12664 final var ma = generateMaCommonAxis();
12665 final var sx = ma.getElementAt(0, 0);
12666 final var sy = ma.getElementAt(1, 1);
12667 final var sz = ma.getElementAt(2, 2);
12668
12669 final var randomizer = new UniformRandomizer();
12670 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12671 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12672 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12673 final var nedPosition = new NEDPosition(latitude, longitude, height);
12674 final var nedVelocity = new NEDVelocity();
12675 final var ecefPosition = new ECEFPosition();
12676 final var ecefVelocity = new ECEFVelocity();
12677 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12678 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12679 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12680 final var gravityNorm = gravity.getNorm();
12681
12682 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
12683 true, bx, by, bz, sx, sy, sz);
12684
12685
12686 assertEquals(biasX, calibrator.getBiasX(), 0.0);
12687 assertEquals(biasY, calibrator.getBiasY(), 0.0);
12688 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12689 final var bx1 = calibrator.getBiasXAsAcceleration();
12690 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12691 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12692 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12693 calibrator.getBiasXAsAcceleration(bx2);
12694 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12695 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12696 final var by1 = calibrator.getBiasYAsAcceleration();
12697 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12698 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12699 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12700 calibrator.getBiasYAsAcceleration(by2);
12701 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12702 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12703 final var bz1 = calibrator.getBiasZAsAcceleration();
12704 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12705 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12706 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12707 calibrator.getBiasZAsAcceleration(bz2);
12708 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12709 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12710 final var biasTriad1 = calibrator.getBiasAsTriad();
12711 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12712 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12713 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12714 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12715 final var biasTriad2 = new AccelerationTriad();
12716 calibrator.getBiasAsTriad(biasTriad2);
12717 assertEquals(biasTriad1, biasTriad2);
12718 assertEquals(sx, calibrator.getInitialSx(), 0.0);
12719 assertEquals(sy, calibrator.getInitialSy(), 0.0);
12720 assertEquals(sz, calibrator.getInitialSz(), 0.0);
12721 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
12722 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
12723 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
12724 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
12725 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
12726 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
12727 final var bias1 = calibrator.getBias();
12728 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12729 final var bias2 = new double[3];
12730 calibrator.getBias(bias2);
12731 assertArrayEquals(bias1, bias2, 0.0);
12732 final var b1 = calibrator.getBiasAsMatrix();
12733 assertEquals(b1, ba);
12734 final var b2 = new Matrix(3, 1);
12735 calibrator.getBiasAsMatrix(b2);
12736 assertEquals(b1, b2);
12737 final var ma1 = calibrator.getInitialMa();
12738 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12739 final var ma2 = new Matrix(3, 3);
12740 calibrator.getInitialMa(ma2);
12741 assertEquals(ma1, ma2);
12742 assertSame(measurements, calibrator.getMeasurements());
12743 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
12744 calibrator.getMeasurementType());
12745 assertFalse(calibrator.isOrderedMeasurementsRequired());
12746 assertFalse(calibrator.isQualityScoresRequired());
12747 assertTrue(calibrator.isCommonAxisUsed());
12748 assertNull(calibrator.getListener());
12749 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
12750 assertFalse(calibrator.isReady());
12751 assertFalse(calibrator.isRunning());
12752 assertNull(calibrator.getEstimatedMa());
12753 assertNull(calibrator.getEstimatedSx());
12754 assertNull(calibrator.getEstimatedSy());
12755 assertNull(calibrator.getEstimatedSz());
12756 assertNull(calibrator.getEstimatedMxy());
12757 assertNull(calibrator.getEstimatedMxz());
12758 assertNull(calibrator.getEstimatedMyx());
12759 assertNull(calibrator.getEstimatedMyz());
12760 assertNull(calibrator.getEstimatedMzx());
12761 assertNull(calibrator.getEstimatedMzy());
12762 assertNull(calibrator.getEstimatedCovariance());
12763 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12764 assertNotNull(calibrator.getGroundTruthGravityNorm());
12765 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12766 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12767 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12768 ABSOLUTE_ERROR));
12769 final var g = new Acceleration(0.0, AccelerationUnit.G);
12770 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12771 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12772 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12773
12774
12775 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12776 -gravityNorm, measurements, true, bx, by, bz, sx, sy, sz));
12777 }
12778
12779 @Test
12780 void testConstructor117() throws WrongSizeException {
12781 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
12782
12783 final var ba = generateBa();
12784 final var biasX = ba.getElementAtIndex(0);
12785 final var biasY = ba.getElementAtIndex(1);
12786 final var biasZ = ba.getElementAtIndex(2);
12787
12788 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12789 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12790 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12791
12792 final var ma = generateMaCommonAxis();
12793 final var sx = ma.getElementAt(0, 0);
12794 final var sy = ma.getElementAt(1, 1);
12795 final var sz = ma.getElementAt(2, 2);
12796
12797 final var randomizer = new UniformRandomizer();
12798 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12799 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12800 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12801 final var nedPosition = new NEDPosition(latitude, longitude, height);
12802 final var nedVelocity = new NEDVelocity();
12803 final var ecefPosition = new ECEFPosition();
12804 final var ecefVelocity = new ECEFVelocity();
12805 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12806 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12807 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12808 final var gravityNorm = gravity.getNorm();
12809
12810 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
12811 true, bx, by, bz, sx, sy, sz, this);
12812
12813
12814 assertEquals(biasX, calibrator.getBiasX(), 0.0);
12815 assertEquals(biasY, calibrator.getBiasY(), 0.0);
12816 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12817 final var bx1 = calibrator.getBiasXAsAcceleration();
12818 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12819 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12820 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12821 calibrator.getBiasXAsAcceleration(bx2);
12822 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12823 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12824 final var by1 = calibrator.getBiasYAsAcceleration();
12825 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12826 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12827 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12828 calibrator.getBiasYAsAcceleration(by2);
12829 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12830 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12831 final var bz1 = calibrator.getBiasZAsAcceleration();
12832 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12833 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12834 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12835 calibrator.getBiasZAsAcceleration(bz2);
12836 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12837 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12838 final var biasTriad1 = calibrator.getBiasAsTriad();
12839 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12840 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12841 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12842 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12843 final var biasTriad2 = new AccelerationTriad();
12844 calibrator.getBiasAsTriad(biasTriad2);
12845 assertEquals(biasTriad1, biasTriad2);
12846 assertEquals(sx, calibrator.getInitialSx(), 0.0);
12847 assertEquals(sy, calibrator.getInitialSy(), 0.0);
12848 assertEquals(sz, calibrator.getInitialSz(), 0.0);
12849 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
12850 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
12851 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
12852 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
12853 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
12854 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
12855 final var bias1 = calibrator.getBias();
12856 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12857 final var bias2 = new double[3];
12858 calibrator.getBias(bias2);
12859 assertArrayEquals(bias1, bias2, 0.0);
12860 final var b1 = calibrator.getBiasAsMatrix();
12861 assertEquals(b1, ba);
12862 final var b2 = new Matrix(3, 1);
12863 calibrator.getBiasAsMatrix(b2);
12864 assertEquals(b1, b2);
12865 final var ma1 = calibrator.getInitialMa();
12866 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12867 final var ma2 = new Matrix(3, 3);
12868 calibrator.getInitialMa(ma2);
12869 assertEquals(ma1, ma2);
12870 assertSame(measurements, calibrator.getMeasurements());
12871 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
12872 calibrator.getMeasurementType());
12873 assertFalse(calibrator.isOrderedMeasurementsRequired());
12874 assertFalse(calibrator.isQualityScoresRequired());
12875 assertTrue(calibrator.isCommonAxisUsed());
12876 assertSame(this, calibrator.getListener());
12877 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
12878 assertFalse(calibrator.isReady());
12879 assertFalse(calibrator.isRunning());
12880 assertNull(calibrator.getEstimatedMa());
12881 assertNull(calibrator.getEstimatedSx());
12882 assertNull(calibrator.getEstimatedSy());
12883 assertNull(calibrator.getEstimatedSz());
12884 assertNull(calibrator.getEstimatedMxy());
12885 assertNull(calibrator.getEstimatedMxz());
12886 assertNull(calibrator.getEstimatedMyx());
12887 assertNull(calibrator.getEstimatedMyz());
12888 assertNull(calibrator.getEstimatedMzx());
12889 assertNull(calibrator.getEstimatedMzy());
12890 assertNull(calibrator.getEstimatedCovariance());
12891 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12892 assertNotNull(calibrator.getGroundTruthGravityNorm());
12893 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12894 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12895 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12896 ABSOLUTE_ERROR));
12897 final var g = new Acceleration(0.0, AccelerationUnit.G);
12898 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12899 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12900 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12901
12902
12903 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12904 -gravityNorm, measurements, true, bx, by, bz, sx, sy, sz, this));
12905 }
12906
12907 @Test
12908 void testConstructor118() throws WrongSizeException {
12909 final var ba = generateBa();
12910 final var biasX = ba.getElementAtIndex(0);
12911 final var biasY = ba.getElementAtIndex(1);
12912 final var biasZ = ba.getElementAtIndex(2);
12913
12914 final var ma = generateMaCommonAxis();
12915 final var sx = ma.getElementAt(0, 0);
12916 final var sy = ma.getElementAt(1, 1);
12917 final var sz = ma.getElementAt(2, 2);
12918 final var mxy = ma.getElementAt(0, 1);
12919 final var mxz = ma.getElementAt(0, 2);
12920 final var myx = ma.getElementAt(1, 0);
12921 final var myz = ma.getElementAt(1, 2);
12922 final var mzx = ma.getElementAt(2, 0);
12923 final var mzy = ma.getElementAt(2, 1);
12924
12925 final var randomizer = new UniformRandomizer();
12926 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12927 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12928 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12929 final var nedPosition = new NEDPosition(latitude, longitude, height);
12930 final var nedVelocity = new NEDVelocity();
12931 final var ecefPosition = new ECEFPosition();
12932 final var ecefVelocity = new ECEFVelocity();
12933 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12934 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12935 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12936 final var gravityNorm = gravity.getNorm();
12937
12938 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ,
12939 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
12940
12941
12942 assertEquals(biasX, calibrator.getBiasX(), 0.0);
12943 assertEquals(biasY, calibrator.getBiasY(), 0.0);
12944 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12945 final var bx1 = calibrator.getBiasXAsAcceleration();
12946 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12947 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12948 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12949 calibrator.getBiasXAsAcceleration(bx2);
12950 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12951 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12952 final var by1 = calibrator.getBiasYAsAcceleration();
12953 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12954 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12955 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12956 calibrator.getBiasYAsAcceleration(by2);
12957 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12958 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12959 final var bz1 = calibrator.getBiasZAsAcceleration();
12960 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12961 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12962 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12963 calibrator.getBiasZAsAcceleration(bz2);
12964 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12965 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12966 final var biasTriad1 = calibrator.getBiasAsTriad();
12967 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12968 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12969 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12970 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12971 final var biasTriad2 = new AccelerationTriad();
12972 calibrator.getBiasAsTriad(biasTriad2);
12973 assertEquals(biasTriad1, biasTriad2);
12974 assertEquals(sx, calibrator.getInitialSx(), 0.0);
12975 assertEquals(sy, calibrator.getInitialSy(), 0.0);
12976 assertEquals(sz, calibrator.getInitialSz(), 0.0);
12977 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
12978 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
12979 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
12980 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
12981 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
12982 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
12983 final var bias1 = calibrator.getBias();
12984 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12985 final var bias2 = new double[3];
12986 calibrator.getBias(bias2);
12987 assertArrayEquals(bias1, bias2, 0.0);
12988 final var b1 = calibrator.getBiasAsMatrix();
12989 assertEquals(b1, ba);
12990 final var b2 = new Matrix(3, 1);
12991 calibrator.getBiasAsMatrix(b2);
12992 assertEquals(b1, b2);
12993 final var ma1 = new Matrix(3, 3);
12994 ma1.setSubmatrix(0, 0, 2, 2,
12995 new double[]{sx, myx, mzx,
12996 mxy, sy, mzy,
12997 mxz, myz, sz});
12998 assertEquals(ma1, calibrator.getInitialMa());
12999 final var ma2 = new Matrix(3, 3);
13000 calibrator.getInitialMa(ma2);
13001 assertEquals(ma1, ma2);
13002 assertNull(calibrator.getMeasurements());
13003 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13004 calibrator.getMeasurementType());
13005 assertFalse(calibrator.isOrderedMeasurementsRequired());
13006 assertFalse(calibrator.isQualityScoresRequired());
13007 assertFalse(calibrator.isCommonAxisUsed());
13008 assertNull(calibrator.getListener());
13009 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
13010 assertFalse(calibrator.isReady());
13011 assertFalse(calibrator.isRunning());
13012 assertNull(calibrator.getEstimatedMa());
13013 assertNull(calibrator.getEstimatedSx());
13014 assertNull(calibrator.getEstimatedSy());
13015 assertNull(calibrator.getEstimatedSz());
13016 assertNull(calibrator.getEstimatedMxy());
13017 assertNull(calibrator.getEstimatedMxz());
13018 assertNull(calibrator.getEstimatedMyx());
13019 assertNull(calibrator.getEstimatedMyz());
13020 assertNull(calibrator.getEstimatedMzx());
13021 assertNull(calibrator.getEstimatedMzy());
13022 assertNull(calibrator.getEstimatedCovariance());
13023 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13024 assertNotNull(calibrator.getGroundTruthGravityNorm());
13025 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13026 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13027 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13028 ABSOLUTE_ERROR));
13029 final var g = new Acceleration(0.0, AccelerationUnit.G);
13030 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13031 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13032 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13033
13034
13035 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13036 -gravityNorm, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
13037 }
13038
13039 @Test
13040 void testConstructor119() throws WrongSizeException {
13041 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
13042
13043 final var ba = generateBa();
13044 final var biasX = ba.getElementAtIndex(0);
13045 final var biasY = ba.getElementAtIndex(1);
13046 final var biasZ = ba.getElementAtIndex(2);
13047
13048 final var ma = generateMaCommonAxis();
13049 final var sx = ma.getElementAt(0, 0);
13050 final var sy = ma.getElementAt(1, 1);
13051 final var sz = ma.getElementAt(2, 2);
13052 final var mxy = ma.getElementAt(0, 1);
13053 final var mxz = ma.getElementAt(0, 2);
13054 final var myx = ma.getElementAt(1, 0);
13055 final var myz = ma.getElementAt(1, 2);
13056 final var mzx = ma.getElementAt(2, 0);
13057 final var mzy = ma.getElementAt(2, 1);
13058
13059 final var randomizer = new UniformRandomizer();
13060 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13061 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13062 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13063 final var nedPosition = new NEDPosition(latitude, longitude, height);
13064 final var nedVelocity = new NEDVelocity();
13065 final var ecefPosition = new ECEFPosition();
13066 final var ecefVelocity = new ECEFVelocity();
13067 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
13068 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13069 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13070 final var gravityNorm = gravity.getNorm();
13071
13072 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
13073 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
13074
13075
13076 assertEquals(biasX, calibrator.getBiasX(), 0.0);
13077 assertEquals(biasY, calibrator.getBiasY(), 0.0);
13078 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
13079 final var bx1 = calibrator.getBiasXAsAcceleration();
13080 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
13081 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
13082 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13083 calibrator.getBiasXAsAcceleration(bx2);
13084 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
13085 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
13086 final var by1 = calibrator.getBiasYAsAcceleration();
13087 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
13088 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
13089 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13090 calibrator.getBiasYAsAcceleration(by2);
13091 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
13092 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
13093 final var bz1 = calibrator.getBiasZAsAcceleration();
13094 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
13095 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
13096 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13097 calibrator.getBiasZAsAcceleration(bz2);
13098 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
13099 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
13100 final var biasTriad1 = calibrator.getBiasAsTriad();
13101 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
13102 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
13103 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
13104 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
13105 final var biasTriad2 = new AccelerationTriad();
13106 calibrator.getBiasAsTriad(biasTriad2);
13107 assertEquals(biasTriad1, biasTriad2);
13108 assertEquals(sx, calibrator.getInitialSx(), 0.0);
13109 assertEquals(sy, calibrator.getInitialSy(), 0.0);
13110 assertEquals(sz, calibrator.getInitialSz(), 0.0);
13111 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
13112 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
13113 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
13114 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
13115 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
13116 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
13117 final var bias1 = calibrator.getBias();
13118 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13119 final var bias2 = new double[3];
13120 calibrator.getBias(bias2);
13121 assertArrayEquals(bias1, bias2, 0.0);
13122 final var b1 = calibrator.getBiasAsMatrix();
13123 assertEquals(b1, ba);
13124 final var b2 = new Matrix(3, 1);
13125 calibrator.getBiasAsMatrix(b2);
13126 assertEquals(b1, b2);
13127 final var ma1 = new Matrix(3, 3);
13128 ma1.setSubmatrix(0, 0, 2, 2,
13129 new double[]{sx, myx, mzx,
13130 mxy, sy, mzy,
13131 mxz, myz, sz});
13132 assertEquals(ma1, calibrator.getInitialMa());
13133 final var ma2 = new Matrix(3, 3);
13134 calibrator.getInitialMa(ma2);
13135 assertEquals(ma1, ma2);
13136 assertSame(measurements, calibrator.getMeasurements());
13137 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13138 calibrator.getMeasurementType());
13139 assertFalse(calibrator.isOrderedMeasurementsRequired());
13140 assertFalse(calibrator.isQualityScoresRequired());
13141 assertFalse(calibrator.isCommonAxisUsed());
13142 assertNull(calibrator.getListener());
13143 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
13144 assertFalse(calibrator.isReady());
13145 assertFalse(calibrator.isRunning());
13146 assertNull(calibrator.getEstimatedMa());
13147 assertNull(calibrator.getEstimatedSx());
13148 assertNull(calibrator.getEstimatedSy());
13149 assertNull(calibrator.getEstimatedSz());
13150 assertNull(calibrator.getEstimatedMxy());
13151 assertNull(calibrator.getEstimatedMxz());
13152 assertNull(calibrator.getEstimatedMyx());
13153 assertNull(calibrator.getEstimatedMyz());
13154 assertNull(calibrator.getEstimatedMzx());
13155 assertNull(calibrator.getEstimatedMzy());
13156 assertNull(calibrator.getEstimatedCovariance());
13157 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13158 assertNotNull(calibrator.getGroundTruthGravityNorm());
13159 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13160 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13161 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13162 ABSOLUTE_ERROR));
13163 final var g = new Acceleration(0.0, AccelerationUnit.G);
13164 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13165 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13166 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13167
13168
13169 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13170 -gravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
13171 }
13172
13173 @Test
13174 void testConstructor120() throws WrongSizeException {
13175 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
13176
13177 final var ba = generateBa();
13178 final var biasX = ba.getElementAtIndex(0);
13179 final var biasY = ba.getElementAtIndex(1);
13180 final var biasZ = ba.getElementAtIndex(2);
13181
13182 final var ma = generateMaCommonAxis();
13183 final var sx = ma.getElementAt(0, 0);
13184 final var sy = ma.getElementAt(1, 1);
13185 final var sz = ma.getElementAt(2, 2);
13186 final var mxy = ma.getElementAt(0, 1);
13187 final var mxz = ma.getElementAt(0, 2);
13188 final var myx = ma.getElementAt(1, 0);
13189 final var myz = ma.getElementAt(1, 2);
13190 final var mzx = ma.getElementAt(2, 0);
13191 final var mzy = ma.getElementAt(2, 1);
13192
13193 final var randomizer = new UniformRandomizer();
13194 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13195 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13196 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13197 final var nedPosition = new NEDPosition(latitude, longitude, height);
13198 final var nedVelocity = new NEDVelocity();
13199 final var ecefPosition = new ECEFPosition();
13200 final var ecefVelocity = new ECEFVelocity();
13201 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
13202 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13203 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13204 final var gravityNorm = gravity.getNorm();
13205
13206 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
13207 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
13208
13209
13210 assertEquals(biasX, calibrator.getBiasX(), 0.0);
13211 assertEquals(biasY, calibrator.getBiasY(), 0.0);
13212 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
13213 final var bx1 = calibrator.getBiasXAsAcceleration();
13214 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
13215 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
13216 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13217 calibrator.getBiasXAsAcceleration(bx2);
13218 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
13219 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
13220 final var by1 = calibrator.getBiasYAsAcceleration();
13221 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
13222 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
13223 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13224 calibrator.getBiasYAsAcceleration(by2);
13225 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
13226 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
13227 final var bz1 = calibrator.getBiasZAsAcceleration();
13228 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
13229 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
13230 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13231 calibrator.getBiasZAsAcceleration(bz2);
13232 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
13233 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
13234 final var biasTriad1 = calibrator.getBiasAsTriad();
13235 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
13236 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
13237 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
13238 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
13239 final var biasTriad2 = new AccelerationTriad();
13240 calibrator.getBiasAsTriad(biasTriad2);
13241 assertEquals(biasTriad1, biasTriad2);
13242 assertEquals(sx, calibrator.getInitialSx(), 0.0);
13243 assertEquals(sy, calibrator.getInitialSy(), 0.0);
13244 assertEquals(sz, calibrator.getInitialSz(), 0.0);
13245 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
13246 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
13247 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
13248 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
13249 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
13250 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
13251 final var bias1 = calibrator.getBias();
13252 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13253 final var bias2 = new double[3];
13254 calibrator.getBias(bias2);
13255 assertArrayEquals(bias1, bias2, 0.0);
13256 final var b1 = calibrator.getBiasAsMatrix();
13257 assertEquals(b1, ba);
13258 final var b2 = new Matrix(3, 1);
13259 calibrator.getBiasAsMatrix(b2);
13260 assertEquals(b1, b2);
13261 final var ma1 = new Matrix(3, 3);
13262 ma1.setSubmatrix(0, 0, 2, 2,
13263 new double[]{sx, myx, mzx,
13264 mxy, sy, mzy,
13265 mxz, myz, sz});
13266 assertEquals(ma1, calibrator.getInitialMa());
13267 final var ma2 = new Matrix(3, 3);
13268 calibrator.getInitialMa(ma2);
13269 assertEquals(ma1, ma2);
13270 assertSame(measurements, calibrator.getMeasurements());
13271 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13272 calibrator.getMeasurementType());
13273 assertFalse(calibrator.isOrderedMeasurementsRequired());
13274 assertFalse(calibrator.isQualityScoresRequired());
13275 assertFalse(calibrator.isCommonAxisUsed());
13276 assertSame(this, calibrator.getListener());
13277 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
13278 assertFalse(calibrator.isReady());
13279 assertFalse(calibrator.isRunning());
13280 assertNull(calibrator.getEstimatedMa());
13281 assertNull(calibrator.getEstimatedSx());
13282 assertNull(calibrator.getEstimatedSy());
13283 assertNull(calibrator.getEstimatedSz());
13284 assertNull(calibrator.getEstimatedMxy());
13285 assertNull(calibrator.getEstimatedMxz());
13286 assertNull(calibrator.getEstimatedMyx());
13287 assertNull(calibrator.getEstimatedMyz());
13288 assertNull(calibrator.getEstimatedMzx());
13289 assertNull(calibrator.getEstimatedMzy());
13290 assertNull(calibrator.getEstimatedCovariance());
13291 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13292 assertNotNull(calibrator.getGroundTruthGravityNorm());
13293 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13294 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13295 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13296 ABSOLUTE_ERROR));
13297 final var g = new Acceleration(0.0, AccelerationUnit.G);
13298 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13299 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13300 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13301
13302
13303 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13304 -gravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
13305 this));
13306 }
13307
13308 @Test
13309 void testConstructor121() throws WrongSizeException {
13310 final var ba = generateBa();
13311 final var biasX = ba.getElementAtIndex(0);
13312 final var biasY = ba.getElementAtIndex(1);
13313 final var biasZ = ba.getElementAtIndex(2);
13314
13315 final var ma = generateMaCommonAxis();
13316 final var sx = ma.getElementAt(0, 0);
13317 final var sy = ma.getElementAt(1, 1);
13318 final var sz = ma.getElementAt(2, 2);
13319 final var mxy = ma.getElementAt(0, 1);
13320 final var mxz = ma.getElementAt(0, 2);
13321 final var myx = ma.getElementAt(1, 0);
13322 final var myz = ma.getElementAt(1, 2);
13323 final var mzx = ma.getElementAt(2, 0);
13324 final var mzy = ma.getElementAt(2, 1);
13325
13326 final var randomizer = new UniformRandomizer();
13327 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13328 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13329 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13330 final var nedPosition = new NEDPosition(latitude, longitude, height);
13331 final var nedVelocity = new NEDVelocity();
13332 final var ecefPosition = new ECEFPosition();
13333 final var ecefVelocity = new ECEFVelocity();
13334 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
13335 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13336 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13337 final var gravityNorm = gravity.getNorm();
13338
13339 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
13340 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
13341
13342
13343 assertEquals(biasX, calibrator.getBiasX(), 0.0);
13344 assertEquals(biasY, calibrator.getBiasY(), 0.0);
13345 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
13346 final var bx1 = calibrator.getBiasXAsAcceleration();
13347 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
13348 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
13349 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13350 calibrator.getBiasXAsAcceleration(bx2);
13351 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
13352 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
13353 final var by1 = calibrator.getBiasYAsAcceleration();
13354 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
13355 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
13356 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13357 calibrator.getBiasYAsAcceleration(by2);
13358 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
13359 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
13360 final var bz1 = calibrator.getBiasZAsAcceleration();
13361 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
13362 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
13363 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13364 calibrator.getBiasZAsAcceleration(bz2);
13365 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
13366 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
13367 final var biasTriad1 = calibrator.getBiasAsTriad();
13368 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
13369 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
13370 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
13371 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
13372 final var biasTriad2 = new AccelerationTriad();
13373 calibrator.getBiasAsTriad(biasTriad2);
13374 assertEquals(biasTriad1, biasTriad2);
13375 assertEquals(sx, calibrator.getInitialSx(), 0.0);
13376 assertEquals(sy, calibrator.getInitialSy(), 0.0);
13377 assertEquals(sz, calibrator.getInitialSz(), 0.0);
13378 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
13379 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
13380 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
13381 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
13382 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
13383 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
13384 final var bias1 = calibrator.getBias();
13385 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13386 final var bias2 = new double[3];
13387 calibrator.getBias(bias2);
13388 assertArrayEquals(bias1, bias2, 0.0);
13389 final var b1 = calibrator.getBiasAsMatrix();
13390 assertEquals(b1, ba);
13391 final var b2 = new Matrix(3, 1);
13392 calibrator.getBiasAsMatrix(b2);
13393 assertEquals(b1, b2);
13394 final var ma1 = new Matrix(3, 3);
13395 ma1.setSubmatrix(0, 0, 2, 2,
13396 new double[]{sx, myx, mzx,
13397 mxy, sy, mzy,
13398 mxz, myz, sz});
13399 assertEquals(ma1, calibrator.getInitialMa());
13400 final var ma2 = new Matrix(3, 3);
13401 calibrator.getInitialMa(ma2);
13402 assertEquals(ma1, ma2);
13403 assertNull(calibrator.getMeasurements());
13404 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13405 calibrator.getMeasurementType());
13406 assertFalse(calibrator.isOrderedMeasurementsRequired());
13407 assertFalse(calibrator.isQualityScoresRequired());
13408 assertTrue(calibrator.isCommonAxisUsed());
13409 assertNull(calibrator.getListener());
13410 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
13411 assertFalse(calibrator.isReady());
13412 assertFalse(calibrator.isRunning());
13413 assertNull(calibrator.getEstimatedMa());
13414 assertNull(calibrator.getEstimatedSx());
13415 assertNull(calibrator.getEstimatedSy());
13416 assertNull(calibrator.getEstimatedSz());
13417 assertNull(calibrator.getEstimatedMxy());
13418 assertNull(calibrator.getEstimatedMxz());
13419 assertNull(calibrator.getEstimatedMyx());
13420 assertNull(calibrator.getEstimatedMyz());
13421 assertNull(calibrator.getEstimatedMzx());
13422 assertNull(calibrator.getEstimatedMzy());
13423 assertNull(calibrator.getEstimatedCovariance());
13424 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13425 assertNotNull(calibrator.getGroundTruthGravityNorm());
13426 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13427 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13428 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13429 ABSOLUTE_ERROR));
13430 final var g = new Acceleration(0.0, AccelerationUnit.G);
13431 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13432 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13433 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13434
13435
13436 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13437 -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
13438 }
13439
13440 @Test
13441 void testConstructor122() throws WrongSizeException {
13442 final var ba = generateBa();
13443 final var biasX = ba.getElementAtIndex(0);
13444 final var biasY = ba.getElementAtIndex(1);
13445 final var biasZ = ba.getElementAtIndex(2);
13446
13447 final var ma = generateMaCommonAxis();
13448 final var sx = ma.getElementAt(0, 0);
13449 final var sy = ma.getElementAt(1, 1);
13450 final var sz = ma.getElementAt(2, 2);
13451 final var mxy = ma.getElementAt(0, 1);
13452 final var mxz = ma.getElementAt(0, 2);
13453 final var myx = ma.getElementAt(1, 0);
13454 final var myz = ma.getElementAt(1, 2);
13455 final var mzx = ma.getElementAt(2, 0);
13456 final var mzy = ma.getElementAt(2, 1);
13457
13458 final var randomizer = new UniformRandomizer();
13459 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13460 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13461 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13462 final var nedPosition = new NEDPosition(latitude, longitude, height);
13463 final var nedVelocity = new NEDVelocity();
13464 final var ecefPosition = new ECEFPosition();
13465 final var ecefVelocity = new ECEFVelocity();
13466 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
13467 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13468 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13469 final var gravityNorm = gravity.getNorm();
13470
13471 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
13472 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
13473
13474
13475 assertEquals(biasX, calibrator.getBiasX(), 0.0);
13476 assertEquals(biasY, calibrator.getBiasY(), 0.0);
13477 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
13478 final var bx1 = calibrator.getBiasXAsAcceleration();
13479 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
13480 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
13481 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13482 calibrator.getBiasXAsAcceleration(bx2);
13483 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
13484 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
13485 final var by1 = calibrator.getBiasYAsAcceleration();
13486 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
13487 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
13488 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13489 calibrator.getBiasYAsAcceleration(by2);
13490 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
13491 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
13492 final var bz1 = calibrator.getBiasZAsAcceleration();
13493 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
13494 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
13495 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13496 calibrator.getBiasZAsAcceleration(bz2);
13497 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
13498 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
13499 final var biasTriad1 = calibrator.getBiasAsTriad();
13500 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
13501 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
13502 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
13503 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
13504 final var biasTriad2 = new AccelerationTriad();
13505 calibrator.getBiasAsTriad(biasTriad2);
13506 assertEquals(biasTriad1, biasTriad2);
13507 assertEquals(sx, calibrator.getInitialSx(), 0.0);
13508 assertEquals(sy, calibrator.getInitialSy(), 0.0);
13509 assertEquals(sz, calibrator.getInitialSz(), 0.0);
13510 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
13511 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
13512 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
13513 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
13514 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
13515 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
13516 final var bias1 = calibrator.getBias();
13517 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13518 final var bias2 = new double[3];
13519 calibrator.getBias(bias2);
13520 assertArrayEquals(bias1, bias2, 0.0);
13521 final var b1 = calibrator.getBiasAsMatrix();
13522 assertEquals(b1, ba);
13523 final var b2 = new Matrix(3, 1);
13524 calibrator.getBiasAsMatrix(b2);
13525 assertEquals(b1, b2);
13526 final var ma1 = new Matrix(3, 3);
13527 ma1.setSubmatrix(0, 0, 2, 2,
13528 new double[]{sx, myx, mzx,
13529 mxy, sy, mzy,
13530 mxz, myz, sz});
13531 assertEquals(ma1, calibrator.getInitialMa());
13532 final var ma2 = new Matrix(3, 3);
13533 calibrator.getInitialMa(ma2);
13534 assertEquals(ma1, ma2);
13535 assertNull(calibrator.getMeasurements());
13536 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13537 calibrator.getMeasurementType());
13538 assertFalse(calibrator.isOrderedMeasurementsRequired());
13539 assertFalse(calibrator.isQualityScoresRequired());
13540 assertTrue(calibrator.isCommonAxisUsed());
13541 assertSame(this, calibrator.getListener());
13542 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
13543 assertFalse(calibrator.isReady());
13544 assertFalse(calibrator.isRunning());
13545 assertNull(calibrator.getEstimatedMa());
13546 assertNull(calibrator.getEstimatedSx());
13547 assertNull(calibrator.getEstimatedSy());
13548 assertNull(calibrator.getEstimatedSz());
13549 assertNull(calibrator.getEstimatedMxy());
13550 assertNull(calibrator.getEstimatedMxz());
13551 assertNull(calibrator.getEstimatedMyx());
13552 assertNull(calibrator.getEstimatedMyz());
13553 assertNull(calibrator.getEstimatedMzx());
13554 assertNull(calibrator.getEstimatedMzy());
13555 assertNull(calibrator.getEstimatedCovariance());
13556 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13557 assertNotNull(calibrator.getGroundTruthGravityNorm());
13558 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13559 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13560 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13561 ABSOLUTE_ERROR));
13562 final var g = new Acceleration(0.0, AccelerationUnit.G);
13563 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13564 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13565 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13566
13567
13568 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13569 -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
13570 }
13571
13572 @Test
13573 void testConstructor123() throws WrongSizeException {
13574 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
13575
13576 final var ba = generateBa();
13577 final var biasX = ba.getElementAtIndex(0);
13578 final var biasY = ba.getElementAtIndex(1);
13579 final var biasZ = ba.getElementAtIndex(2);
13580
13581 final var ma = generateMaCommonAxis();
13582 final var sx = ma.getElementAt(0, 0);
13583 final var sy = ma.getElementAt(1, 1);
13584 final var sz = ma.getElementAt(2, 2);
13585 final var mxy = ma.getElementAt(0, 1);
13586 final var mxz = ma.getElementAt(0, 2);
13587 final var myx = ma.getElementAt(1, 0);
13588 final var myz = ma.getElementAt(1, 2);
13589 final var mzx = ma.getElementAt(2, 0);
13590 final var mzy = ma.getElementAt(2, 1);
13591
13592 final var randomizer = new UniformRandomizer();
13593 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13594 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13595 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13596 final var nedPosition = new NEDPosition(latitude, longitude, height);
13597 final var nedVelocity = new NEDVelocity();
13598 final var ecefPosition = new ECEFPosition();
13599 final var ecefVelocity = new ECEFVelocity();
13600 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
13601 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13602 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13603 final var gravityNorm = gravity.getNorm();
13604
13605 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
13606 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
13607
13608
13609 assertEquals(biasX, calibrator.getBiasX(), 0.0);
13610 assertEquals(biasY, calibrator.getBiasY(), 0.0);
13611 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
13612 final var bx1 = calibrator.getBiasXAsAcceleration();
13613 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
13614 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
13615 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13616 calibrator.getBiasXAsAcceleration(bx2);
13617 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
13618 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
13619 final var by1 = calibrator.getBiasYAsAcceleration();
13620 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
13621 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
13622 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13623 calibrator.getBiasYAsAcceleration(by2);
13624 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
13625 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
13626 final var bz1 = calibrator.getBiasZAsAcceleration();
13627 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
13628 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
13629 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13630 calibrator.getBiasZAsAcceleration(bz2);
13631 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
13632 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
13633 final var biasTriad1 = calibrator.getBiasAsTriad();
13634 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
13635 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
13636 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
13637 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
13638 final var biasTriad2 = new AccelerationTriad();
13639 calibrator.getBiasAsTriad(biasTriad2);
13640 assertEquals(biasTriad1, biasTriad2);
13641 assertEquals(sx, calibrator.getInitialSx(), 0.0);
13642 assertEquals(sy, calibrator.getInitialSy(), 0.0);
13643 assertEquals(sz, calibrator.getInitialSz(), 0.0);
13644 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
13645 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
13646 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
13647 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
13648 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
13649 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
13650 final var bias1 = calibrator.getBias();
13651 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13652 final var bias2 = new double[3];
13653 calibrator.getBias(bias2);
13654 assertArrayEquals(bias1, bias2, 0.0);
13655 final var b1 = calibrator.getBiasAsMatrix();
13656 assertEquals(b1, ba);
13657 final var b2 = new Matrix(3, 1);
13658 calibrator.getBiasAsMatrix(b2);
13659 assertEquals(b1, b2);
13660 final var ma1 = new Matrix(3, 3);
13661 ma1.setSubmatrix(0, 0, 2, 2,
13662 new double[]{sx, myx, mzx,
13663 mxy, sy, mzy,
13664 mxz, myz, sz});
13665 assertEquals(ma1, calibrator.getInitialMa());
13666 final var ma2 = new Matrix(3, 3);
13667 calibrator.getInitialMa(ma2);
13668 assertEquals(ma1, ma2);
13669 assertSame(measurements, calibrator.getMeasurements());
13670 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13671 calibrator.getMeasurementType());
13672 assertFalse(calibrator.isOrderedMeasurementsRequired());
13673 assertFalse(calibrator.isQualityScoresRequired());
13674 assertTrue(calibrator.isCommonAxisUsed());
13675 assertNull(calibrator.getListener());
13676 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
13677 assertFalse(calibrator.isReady());
13678 assertFalse(calibrator.isRunning());
13679 assertNull(calibrator.getEstimatedMa());
13680 assertNull(calibrator.getEstimatedSx());
13681 assertNull(calibrator.getEstimatedSy());
13682 assertNull(calibrator.getEstimatedSz());
13683 assertNull(calibrator.getEstimatedMxy());
13684 assertNull(calibrator.getEstimatedMxz());
13685 assertNull(calibrator.getEstimatedMyx());
13686 assertNull(calibrator.getEstimatedMyz());
13687 assertNull(calibrator.getEstimatedMzx());
13688 assertNull(calibrator.getEstimatedMzy());
13689 assertNull(calibrator.getEstimatedCovariance());
13690 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13691 assertNotNull(calibrator.getGroundTruthGravityNorm());
13692 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13693 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13694 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13695 ABSOLUTE_ERROR));
13696 final var g = new Acceleration(0.0, AccelerationUnit.G);
13697 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13698 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13699 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13700
13701
13702 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13703 -gravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
13704 myz, mzx, mzy));
13705 }
13706
13707 @Test
13708 void testConstructor124() throws WrongSizeException {
13709 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
13710
13711 final var ba = generateBa();
13712 final var biasX = ba.getElementAtIndex(0);
13713 final var biasY = ba.getElementAtIndex(1);
13714 final var biasZ = ba.getElementAtIndex(2);
13715
13716 final var ma = generateMaCommonAxis();
13717 final var sx = ma.getElementAt(0, 0);
13718 final var sy = ma.getElementAt(1, 1);
13719 final var sz = ma.getElementAt(2, 2);
13720 final var mxy = ma.getElementAt(0, 1);
13721 final var mxz = ma.getElementAt(0, 2);
13722 final var myx = ma.getElementAt(1, 0);
13723 final var myz = ma.getElementAt(1, 2);
13724 final var mzx = ma.getElementAt(2, 0);
13725 final var mzy = ma.getElementAt(2, 1);
13726
13727 final var randomizer = new UniformRandomizer();
13728 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13729 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13730 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13731 final var nedPosition = new NEDPosition(latitude, longitude, height);
13732 final var nedVelocity = new NEDVelocity();
13733 final var ecefPosition = new ECEFPosition();
13734 final var ecefVelocity = new ECEFVelocity();
13735 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
13736 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13737 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13738 final var gravityNorm = gravity.getNorm();
13739
13740 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
13741 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
13742
13743
13744 assertEquals(biasX, calibrator.getBiasX(), 0.0);
13745 assertEquals(biasY, calibrator.getBiasY(), 0.0);
13746 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
13747 final var bx1 = calibrator.getBiasXAsAcceleration();
13748 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
13749 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
13750 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13751 calibrator.getBiasXAsAcceleration(bx2);
13752 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
13753 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
13754 final var by1 = calibrator.getBiasYAsAcceleration();
13755 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
13756 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
13757 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13758 calibrator.getBiasYAsAcceleration(by2);
13759 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
13760 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
13761 final var bz1 = calibrator.getBiasZAsAcceleration();
13762 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
13763 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
13764 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13765 calibrator.getBiasZAsAcceleration(bz2);
13766 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
13767 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
13768 final var biasTriad1 = calibrator.getBiasAsTriad();
13769 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
13770 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
13771 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
13772 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
13773 final var biasTriad2 = new AccelerationTriad();
13774 calibrator.getBiasAsTriad(biasTriad2);
13775 assertEquals(biasTriad1, biasTriad2);
13776 assertEquals(sx, calibrator.getInitialSx(), 0.0);
13777 assertEquals(sy, calibrator.getInitialSy(), 0.0);
13778 assertEquals(sz, calibrator.getInitialSz(), 0.0);
13779 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
13780 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
13781 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
13782 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
13783 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
13784 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
13785 final var bias1 = calibrator.getBias();
13786 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13787 final var bias2 = new double[3];
13788 calibrator.getBias(bias2);
13789 assertArrayEquals(bias1, bias2, 0.0);
13790 final var b1 = calibrator.getBiasAsMatrix();
13791 assertEquals(b1, ba);
13792 final var b2 = new Matrix(3, 1);
13793 calibrator.getBiasAsMatrix(b2);
13794 assertEquals(b1, b2);
13795 final var ma1 = new Matrix(3, 3);
13796 ma1.setSubmatrix(0, 0, 2, 2,
13797 new double[]{sx, myx, mzx,
13798 mxy, sy, mzy,
13799 mxz, myz, sz});
13800 assertEquals(ma1, calibrator.getInitialMa());
13801 final var ma2 = new Matrix(3, 3);
13802 calibrator.getInitialMa(ma2);
13803 assertEquals(ma1, ma2);
13804 assertSame(measurements, calibrator.getMeasurements());
13805 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13806 calibrator.getMeasurementType());
13807 assertFalse(calibrator.isOrderedMeasurementsRequired());
13808 assertFalse(calibrator.isQualityScoresRequired());
13809 assertTrue(calibrator.isCommonAxisUsed());
13810 assertSame(this, calibrator.getListener());
13811 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
13812 assertFalse(calibrator.isReady());
13813 assertFalse(calibrator.isRunning());
13814 assertNull(calibrator.getEstimatedMa());
13815 assertNull(calibrator.getEstimatedSx());
13816 assertNull(calibrator.getEstimatedSy());
13817 assertNull(calibrator.getEstimatedSz());
13818 assertNull(calibrator.getEstimatedMxy());
13819 assertNull(calibrator.getEstimatedMxz());
13820 assertNull(calibrator.getEstimatedMyx());
13821 assertNull(calibrator.getEstimatedMyz());
13822 assertNull(calibrator.getEstimatedMzx());
13823 assertNull(calibrator.getEstimatedMzy());
13824 assertNull(calibrator.getEstimatedCovariance());
13825 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13826 assertNotNull(calibrator.getGroundTruthGravityNorm());
13827 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13828 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13829 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13830 ABSOLUTE_ERROR));
13831 final var g = new Acceleration(0.0, AccelerationUnit.G);
13832 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13833 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13834 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13835
13836
13837 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13838 -gravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
13839 }
13840
13841 @Test
13842 void testConstructor125() throws WrongSizeException {
13843 final var ba = generateBa();
13844 final var biasX = ba.getElementAtIndex(0);
13845 final var biasY = ba.getElementAtIndex(1);
13846 final var biasZ = ba.getElementAtIndex(2);
13847
13848 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
13849 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
13850 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
13851
13852 final var ma = generateMaCommonAxis();
13853 final var sx = ma.getElementAt(0, 0);
13854 final var sy = ma.getElementAt(1, 1);
13855 final var sz = ma.getElementAt(2, 2);
13856 final var mxy = ma.getElementAt(0, 1);
13857 final var mxz = ma.getElementAt(0, 2);
13858 final var myx = ma.getElementAt(1, 0);
13859 final var myz = ma.getElementAt(1, 2);
13860 final var mzx = ma.getElementAt(2, 0);
13861 final var mzy = ma.getElementAt(2, 1);
13862
13863 final var randomizer = new UniformRandomizer();
13864 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13865 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13866 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13867 final var nedPosition = new NEDPosition(latitude, longitude, height);
13868 final var nedVelocity = new NEDVelocity();
13869 final var ecefPosition = new ECEFPosition();
13870 final var ecefVelocity = new ECEFVelocity();
13871 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
13872 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13873 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13874 final var gravityNorm = gravity.getNorm();
13875
13876 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz,
13877 mxy, mxz, myx, myz, mzx, mzy);
13878
13879
13880 assertEquals(biasX, calibrator.getBiasX(), 0.0);
13881 assertEquals(biasY, calibrator.getBiasY(), 0.0);
13882 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
13883 final var bx1 = calibrator.getBiasXAsAcceleration();
13884 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
13885 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
13886 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13887 calibrator.getBiasXAsAcceleration(bx2);
13888 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
13889 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
13890 final var by1 = calibrator.getBiasYAsAcceleration();
13891 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
13892 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
13893 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13894 calibrator.getBiasYAsAcceleration(by2);
13895 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
13896 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
13897 final var bz1 = calibrator.getBiasZAsAcceleration();
13898 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
13899 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
13900 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13901 calibrator.getBiasZAsAcceleration(bz2);
13902 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
13903 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
13904 final var biasTriad1 = calibrator.getBiasAsTriad();
13905 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
13906 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
13907 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
13908 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
13909 final var biasTriad2 = new AccelerationTriad();
13910 calibrator.getBiasAsTriad(biasTriad2);
13911 assertEquals(biasTriad1, biasTriad2);
13912 assertEquals(sx, calibrator.getInitialSx(), 0.0);
13913 assertEquals(sy, calibrator.getInitialSy(), 0.0);
13914 assertEquals(sz, calibrator.getInitialSz(), 0.0);
13915 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
13916 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
13917 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
13918 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
13919 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
13920 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
13921 final var bias1 = calibrator.getBias();
13922 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13923 final var bias2 = new double[3];
13924 calibrator.getBias(bias2);
13925 assertArrayEquals(bias1, bias2, 0.0);
13926 final var b1 = calibrator.getBiasAsMatrix();
13927 assertEquals(b1, ba);
13928 final var b2 = new Matrix(3, 1);
13929 calibrator.getBiasAsMatrix(b2);
13930 assertEquals(b1, b2);
13931 final var ma1 = new Matrix(3, 3);
13932 ma1.setSubmatrix(0, 0, 2, 2,
13933 new double[]{sx, myx, mzx,
13934 mxy, sy, mzy,
13935 mxz, myz, sz});
13936 assertEquals(ma1, calibrator.getInitialMa());
13937 final var ma2 = new Matrix(3, 3);
13938 calibrator.getInitialMa(ma2);
13939 assertEquals(ma1, ma2);
13940 assertNull(calibrator.getMeasurements());
13941 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13942 calibrator.getMeasurementType());
13943 assertFalse(calibrator.isOrderedMeasurementsRequired());
13944 assertFalse(calibrator.isQualityScoresRequired());
13945 assertFalse(calibrator.isCommonAxisUsed());
13946 assertNull(calibrator.getListener());
13947 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
13948 assertFalse(calibrator.isReady());
13949 assertFalse(calibrator.isRunning());
13950 assertNull(calibrator.getEstimatedMa());
13951 assertNull(calibrator.getEstimatedSx());
13952 assertNull(calibrator.getEstimatedSy());
13953 assertNull(calibrator.getEstimatedSz());
13954 assertNull(calibrator.getEstimatedMxy());
13955 assertNull(calibrator.getEstimatedMxz());
13956 assertNull(calibrator.getEstimatedMyx());
13957 assertNull(calibrator.getEstimatedMyz());
13958 assertNull(calibrator.getEstimatedMzx());
13959 assertNull(calibrator.getEstimatedMzy());
13960 assertNull(calibrator.getEstimatedCovariance());
13961 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13962 assertNotNull(calibrator.getGroundTruthGravityNorm());
13963 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13964 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13965 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13966 ABSOLUTE_ERROR));
13967 final var g = new Acceleration(0.0, AccelerationUnit.G);
13968 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13969 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13970 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13971
13972
13973 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13974 -gravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
13975 }
13976
13977 @Test
13978 void testConstructor126() throws WrongSizeException {
13979 final var ba = generateBa();
13980 final var biasX = ba.getElementAtIndex(0);
13981 final var biasY = ba.getElementAtIndex(1);
13982 final var biasZ = ba.getElementAtIndex(2);
13983
13984 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
13985 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
13986 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
13987
13988 final var ma = generateMaCommonAxis();
13989 final var sx = ma.getElementAt(0, 0);
13990 final var sy = ma.getElementAt(1, 1);
13991 final var sz = ma.getElementAt(2, 2);
13992 final var mxy = ma.getElementAt(0, 1);
13993 final var mxz = ma.getElementAt(0, 2);
13994 final var myx = ma.getElementAt(1, 0);
13995 final var myz = ma.getElementAt(1, 2);
13996 final var mzx = ma.getElementAt(2, 0);
13997 final var mzy = ma.getElementAt(2, 1);
13998
13999 final var randomizer = new UniformRandomizer();
14000 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14001 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14002 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14003 final var nedPosition = new NEDPosition(latitude, longitude, height);
14004 final var nedVelocity = new NEDVelocity();
14005 final var ecefPosition = new ECEFPosition();
14006 final var ecefVelocity = new ECEFVelocity();
14007 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14008 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14009 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14010 final var gravityNorm = gravity.getNorm();
14011
14012 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz,
14013 mxy, mxz, myx, myz, mzx, mzy, this);
14014
14015
14016 assertEquals(biasX, calibrator.getBiasX(), 0.0);
14017 assertEquals(biasY, calibrator.getBiasY(), 0.0);
14018 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14019 final var bx1 = calibrator.getBiasXAsAcceleration();
14020 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14021 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14022 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14023 calibrator.getBiasXAsAcceleration(bx2);
14024 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14025 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14026 final var by1 = calibrator.getBiasYAsAcceleration();
14027 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14028 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14029 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14030 calibrator.getBiasYAsAcceleration(by2);
14031 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14032 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14033 final var bz1 = calibrator.getBiasZAsAcceleration();
14034 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14035 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14036 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14037 calibrator.getBiasZAsAcceleration(bz2);
14038 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14039 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14040 final var biasTriad1 = calibrator.getBiasAsTriad();
14041 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14042 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14043 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14044 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14045 final var biasTriad2 = new AccelerationTriad();
14046 calibrator.getBiasAsTriad(biasTriad2);
14047 assertEquals(biasTriad1, biasTriad2);
14048 assertEquals(sx, calibrator.getInitialSx(), 0.0);
14049 assertEquals(sy, calibrator.getInitialSy(), 0.0);
14050 assertEquals(sz, calibrator.getInitialSz(), 0.0);
14051 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
14052 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
14053 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
14054 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
14055 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
14056 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
14057 final var bias1 = calibrator.getBias();
14058 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14059 final var bias2 = new double[3];
14060 calibrator.getBias(bias2);
14061 assertArrayEquals(bias1, bias2, 0.0);
14062 final var b1 = calibrator.getBiasAsMatrix();
14063 assertEquals(b1, ba);
14064 final var b2 = new Matrix(3, 1);
14065 calibrator.getBiasAsMatrix(b2);
14066 assertEquals(b1, b2);
14067 final var ma1 = new Matrix(3, 3);
14068 ma1.setSubmatrix(0, 0, 2, 2,
14069 new double[]{sx, myx, mzx,
14070 mxy, sy, mzy,
14071 mxz, myz, sz});
14072 assertEquals(ma1, calibrator.getInitialMa());
14073 final var ma2 = new Matrix(3, 3);
14074 calibrator.getInitialMa(ma2);
14075 assertEquals(ma1, ma2);
14076 assertNull(calibrator.getMeasurements());
14077 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
14078 calibrator.getMeasurementType());
14079 assertFalse(calibrator.isOrderedMeasurementsRequired());
14080 assertFalse(calibrator.isQualityScoresRequired());
14081 assertFalse(calibrator.isCommonAxisUsed());
14082 assertSame(this, calibrator.getListener());
14083 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
14084 assertFalse(calibrator.isReady());
14085 assertFalse(calibrator.isRunning());
14086 assertNull(calibrator.getEstimatedMa());
14087 assertNull(calibrator.getEstimatedSx());
14088 assertNull(calibrator.getEstimatedSy());
14089 assertNull(calibrator.getEstimatedSz());
14090 assertNull(calibrator.getEstimatedMxy());
14091 assertNull(calibrator.getEstimatedMxz());
14092 assertNull(calibrator.getEstimatedMyx());
14093 assertNull(calibrator.getEstimatedMyz());
14094 assertNull(calibrator.getEstimatedMzx());
14095 assertNull(calibrator.getEstimatedMzy());
14096 assertNull(calibrator.getEstimatedCovariance());
14097 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
14098 assertNotNull(calibrator.getGroundTruthGravityNorm());
14099 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14100 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14101 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
14102 ABSOLUTE_ERROR));
14103 final var g = new Acceleration(0.0, AccelerationUnit.G);
14104 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14105 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14106 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
14107
14108
14109 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
14110 -gravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
14111 }
14112
14113 @Test
14114 void testConstructor127() throws WrongSizeException {
14115 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
14116
14117 final var ba = generateBa();
14118 final var biasX = ba.getElementAtIndex(0);
14119 final var biasY = ba.getElementAtIndex(1);
14120 final var biasZ = ba.getElementAtIndex(2);
14121
14122 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14123 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14124 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14125
14126 final var ma = generateMaCommonAxis();
14127 final var sx = ma.getElementAt(0, 0);
14128 final var sy = ma.getElementAt(1, 1);
14129 final var sz = ma.getElementAt(2, 2);
14130 final var mxy = ma.getElementAt(0, 1);
14131 final var mxz = ma.getElementAt(0, 2);
14132 final var myx = ma.getElementAt(1, 0);
14133 final var myz = ma.getElementAt(1, 2);
14134 final var mzx = ma.getElementAt(2, 0);
14135 final var mzy = ma.getElementAt(2, 1);
14136
14137 final var randomizer = new UniformRandomizer();
14138 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14139 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14140 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14141 final var nedPosition = new NEDPosition(latitude, longitude, height);
14142 final var nedVelocity = new NEDVelocity();
14143 final var ecefPosition = new ECEFPosition();
14144 final var ecefVelocity = new ECEFVelocity();
14145 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14146 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14147 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14148 final var gravityNorm = gravity.getNorm();
14149
14150 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
14151 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
14152
14153
14154 assertEquals(biasX, calibrator.getBiasX(), 0.0);
14155 assertEquals(biasY, calibrator.getBiasY(), 0.0);
14156 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14157 final var bx1 = calibrator.getBiasXAsAcceleration();
14158 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14159 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14160 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14161 calibrator.getBiasXAsAcceleration(bx2);
14162 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14163 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14164 final var by1 = calibrator.getBiasYAsAcceleration();
14165 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14166 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14167 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14168 calibrator.getBiasYAsAcceleration(by2);
14169 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14170 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14171 final var bz1 = calibrator.getBiasZAsAcceleration();
14172 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14173 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14174 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14175 calibrator.getBiasZAsAcceleration(bz2);
14176 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14177 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14178 final var biasTriad1 = calibrator.getBiasAsTriad();
14179 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14180 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14181 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14182 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14183 final var biasTriad2 = new AccelerationTriad();
14184 calibrator.getBiasAsTriad(biasTriad2);
14185 assertEquals(biasTriad1, biasTriad2);
14186 assertEquals(sx, calibrator.getInitialSx(), 0.0);
14187 assertEquals(sy, calibrator.getInitialSy(), 0.0);
14188 assertEquals(sz, calibrator.getInitialSz(), 0.0);
14189 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
14190 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
14191 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
14192 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
14193 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
14194 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
14195 final var bias1 = calibrator.getBias();
14196 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14197 final var bias2 = new double[3];
14198 calibrator.getBias(bias2);
14199 assertArrayEquals(bias1, bias2, 0.0);
14200 final var b1 = calibrator.getBiasAsMatrix();
14201 assertEquals(b1, ba);
14202 final var b2 = new Matrix(3, 1);
14203 calibrator.getBiasAsMatrix(b2);
14204 assertEquals(b1, b2);
14205 final var ma1 = new Matrix(3, 3);
14206 ma1.setSubmatrix(0, 0, 2, 2,
14207 new double[]{sx, myx, mzx,
14208 mxy, sy, mzy,
14209 mxz, myz, sz});
14210 assertEquals(ma1, calibrator.getInitialMa());
14211 final var ma2 = new Matrix(3, 3);
14212 calibrator.getInitialMa(ma2);
14213 assertEquals(ma1, ma2);
14214 assertSame(measurements, calibrator.getMeasurements());
14215 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
14216 calibrator.getMeasurementType());
14217 assertFalse(calibrator.isOrderedMeasurementsRequired());
14218 assertFalse(calibrator.isQualityScoresRequired());
14219 assertFalse(calibrator.isCommonAxisUsed());
14220 assertNull(calibrator.getListener());
14221 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
14222 assertFalse(calibrator.isReady());
14223 assertFalse(calibrator.isRunning());
14224 assertNull(calibrator.getEstimatedMa());
14225 assertNull(calibrator.getEstimatedSx());
14226 assertNull(calibrator.getEstimatedSy());
14227 assertNull(calibrator.getEstimatedSz());
14228 assertNull(calibrator.getEstimatedMxy());
14229 assertNull(calibrator.getEstimatedMxz());
14230 assertNull(calibrator.getEstimatedMyx());
14231 assertNull(calibrator.getEstimatedMyz());
14232 assertNull(calibrator.getEstimatedMzx());
14233 assertNull(calibrator.getEstimatedMzy());
14234 assertNull(calibrator.getEstimatedCovariance());
14235 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
14236 assertNotNull(calibrator.getGroundTruthGravityNorm());
14237 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14238 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14239 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
14240 ABSOLUTE_ERROR));
14241 final var g = new Acceleration(0.0, AccelerationUnit.G);
14242 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14243 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14244 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
14245
14246
14247 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
14248 -gravityNorm, measurements, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
14249 }
14250
14251 @Test
14252 void testConstructor128() throws WrongSizeException {
14253 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
14254
14255 final var ba = generateBa();
14256 final var biasX = ba.getElementAtIndex(0);
14257 final var biasY = ba.getElementAtIndex(1);
14258 final var biasZ = ba.getElementAtIndex(2);
14259
14260 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14261 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14262 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14263
14264 final var ma = generateMaCommonAxis();
14265 final var sx = ma.getElementAt(0, 0);
14266 final var sy = ma.getElementAt(1, 1);
14267 final var sz = ma.getElementAt(2, 2);
14268 final var mxy = ma.getElementAt(0, 1);
14269 final var mxz = ma.getElementAt(0, 2);
14270 final var myx = ma.getElementAt(1, 0);
14271 final var myz = ma.getElementAt(1, 2);
14272 final var mzx = ma.getElementAt(2, 0);
14273 final var mzy = ma.getElementAt(2, 1);
14274
14275 final var randomizer = new UniformRandomizer();
14276 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14277 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14278 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14279 final var nedPosition = new NEDPosition(latitude, longitude, height);
14280 final var nedVelocity = new NEDVelocity();
14281 final var ecefPosition = new ECEFPosition();
14282 final var ecefVelocity = new ECEFVelocity();
14283 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14284 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14285 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14286 final var gravityNorm = gravity.getNorm();
14287
14288 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
14289 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
14290
14291
14292 assertEquals(biasX, calibrator.getBiasX(), 0.0);
14293 assertEquals(biasY, calibrator.getBiasY(), 0.0);
14294 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14295 final var bx1 = calibrator.getBiasXAsAcceleration();
14296 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14297 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14298 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14299 calibrator.getBiasXAsAcceleration(bx2);
14300 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14301 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14302 final var by1 = calibrator.getBiasYAsAcceleration();
14303 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14304 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14305 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14306 calibrator.getBiasYAsAcceleration(by2);
14307 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14308 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14309 final var bz1 = calibrator.getBiasZAsAcceleration();
14310 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14311 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14312 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14313 calibrator.getBiasZAsAcceleration(bz2);
14314 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14315 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14316 final var biasTriad1 = calibrator.getBiasAsTriad();
14317 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14318 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14319 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14320 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14321 final var biasTriad2 = new AccelerationTriad();
14322 calibrator.getBiasAsTriad(biasTriad2);
14323 assertEquals(biasTriad1, biasTriad2);
14324 assertEquals(sx, calibrator.getInitialSx(), 0.0);
14325 assertEquals(sy, calibrator.getInitialSy(), 0.0);
14326 assertEquals(sz, calibrator.getInitialSz(), 0.0);
14327 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
14328 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
14329 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
14330 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
14331 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
14332 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
14333 final var bias1 = calibrator.getBias();
14334 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14335 final var bias2 = new double[3];
14336 calibrator.getBias(bias2);
14337 assertArrayEquals(bias1, bias2, 0.0);
14338 final var b1 = calibrator.getBiasAsMatrix();
14339 assertEquals(b1, ba);
14340 final var b2 = new Matrix(3, 1);
14341 calibrator.getBiasAsMatrix(b2);
14342 assertEquals(b1, b2);
14343 final var ma1 = new Matrix(3, 3);
14344 ma1.setSubmatrix(0, 0, 2, 2,
14345 new double[]{sx, myx, mzx,
14346 mxy, sy, mzy,
14347 mxz, myz, sz});
14348 assertEquals(ma1, calibrator.getInitialMa());
14349 final var ma2 = new Matrix(3, 3);
14350 calibrator.getInitialMa(ma2);
14351 assertEquals(ma1, ma2);
14352 assertSame(measurements, calibrator.getMeasurements());
14353 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
14354 calibrator.getMeasurementType());
14355 assertFalse(calibrator.isOrderedMeasurementsRequired());
14356 assertFalse(calibrator.isQualityScoresRequired());
14357 assertFalse(calibrator.isCommonAxisUsed());
14358 assertSame(this, calibrator.getListener());
14359 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
14360 assertFalse(calibrator.isReady());
14361 assertFalse(calibrator.isRunning());
14362 assertNull(calibrator.getEstimatedMa());
14363 assertNull(calibrator.getEstimatedSx());
14364 assertNull(calibrator.getEstimatedSy());
14365 assertNull(calibrator.getEstimatedSz());
14366 assertNull(calibrator.getEstimatedMxy());
14367 assertNull(calibrator.getEstimatedMxz());
14368 assertNull(calibrator.getEstimatedMyx());
14369 assertNull(calibrator.getEstimatedMyz());
14370 assertNull(calibrator.getEstimatedMzx());
14371 assertNull(calibrator.getEstimatedMzy());
14372 assertNull(calibrator.getEstimatedCovariance());
14373 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
14374 assertNotNull(calibrator.getGroundTruthGravityNorm());
14375 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14376 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14377 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(
14378 gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14379 final var g = new Acceleration(0.0, AccelerationUnit.G);
14380 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14381 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14382 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
14383
14384
14385 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
14386 -gravityNorm, measurements, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
14387 }
14388
14389 @Test
14390 void testConstructor129() throws WrongSizeException {
14391 final var ba = generateBa();
14392 final var biasX = ba.getElementAtIndex(0);
14393 final var biasY = ba.getElementAtIndex(1);
14394 final var biasZ = ba.getElementAtIndex(2);
14395
14396 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14397 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14398 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14399
14400 final var ma = generateMaCommonAxis();
14401 final var sx = ma.getElementAt(0, 0);
14402 final var sy = ma.getElementAt(1, 1);
14403 final var sz = ma.getElementAt(2, 2);
14404 final var mxy = ma.getElementAt(0, 1);
14405 final var mxz = ma.getElementAt(0, 2);
14406 final var myx = ma.getElementAt(1, 0);
14407 final var myz = ma.getElementAt(1, 2);
14408 final var mzx = ma.getElementAt(2, 0);
14409 final var mzy = ma.getElementAt(2, 1);
14410
14411 final var randomizer = new UniformRandomizer();
14412 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14413 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14414 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14415 final var nedPosition = new NEDPosition(latitude, longitude, height);
14416 final var nedVelocity = new NEDVelocity();
14417 final var ecefPosition = new ECEFPosition();
14418 final var ecefVelocity = new ECEFVelocity();
14419 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14420 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14421 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14422 final var gravityNorm = gravity.getNorm();
14423
14424 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
14425 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
14426
14427
14428 assertEquals(biasX, calibrator.getBiasX(), 0.0);
14429 assertEquals(biasY, calibrator.getBiasY(), 0.0);
14430 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14431 final var bx1 = calibrator.getBiasXAsAcceleration();
14432 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14433 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14434 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14435 calibrator.getBiasXAsAcceleration(bx2);
14436 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14437 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14438 final var by1 = calibrator.getBiasYAsAcceleration();
14439 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14440 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14441 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14442 calibrator.getBiasYAsAcceleration(by2);
14443 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14444 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14445 final var bz1 = calibrator.getBiasZAsAcceleration();
14446 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14447 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14448 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14449 calibrator.getBiasZAsAcceleration(bz2);
14450 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14451 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14452 final var biasTriad1 = calibrator.getBiasAsTriad();
14453 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14454 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14455 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14456 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14457 final var biasTriad2 = new AccelerationTriad();
14458 calibrator.getBiasAsTriad(biasTriad2);
14459 assertEquals(biasTriad1, biasTriad2);
14460 assertEquals(sx, calibrator.getInitialSx(), 0.0);
14461 assertEquals(sy, calibrator.getInitialSy(), 0.0);
14462 assertEquals(sz, calibrator.getInitialSz(), 0.0);
14463 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
14464 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
14465 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
14466 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
14467 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
14468 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
14469 final var bias1 = calibrator.getBias();
14470 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14471 final var bias2 = new double[3];
14472 calibrator.getBias(bias2);
14473 assertArrayEquals(bias1, bias2, 0.0);
14474 final var b1 = calibrator.getBiasAsMatrix();
14475 assertEquals(b1, ba);
14476 final var b2 = new Matrix(3, 1);
14477 calibrator.getBiasAsMatrix(b2);
14478 assertEquals(b1, b2);
14479 final var ma1 = new Matrix(3, 3);
14480 ma1.setSubmatrix(0, 0, 2, 2,
14481 new double[]{sx, myx, mzx,
14482 mxy, sy, mzy,
14483 mxz, myz, sz});
14484 assertEquals(calibrator.getInitialMa(), ma1);
14485 final var ma2 = new Matrix(3, 3);
14486 calibrator.getInitialMa(ma2);
14487 assertEquals(ma1, ma2);
14488 assertNull(calibrator.getMeasurements());
14489 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
14490 calibrator.getMeasurementType());
14491 assertFalse(calibrator.isOrderedMeasurementsRequired());
14492 assertFalse(calibrator.isQualityScoresRequired());
14493 assertTrue(calibrator.isCommonAxisUsed());
14494 assertNull(calibrator.getListener());
14495 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
14496 assertFalse(calibrator.isReady());
14497 assertFalse(calibrator.isRunning());
14498 assertNull(calibrator.getEstimatedMa());
14499 assertNull(calibrator.getEstimatedSx());
14500 assertNull(calibrator.getEstimatedSy());
14501 assertNull(calibrator.getEstimatedSz());
14502 assertNull(calibrator.getEstimatedMxy());
14503 assertNull(calibrator.getEstimatedMxz());
14504 assertNull(calibrator.getEstimatedMyx());
14505 assertNull(calibrator.getEstimatedMyz());
14506 assertNull(calibrator.getEstimatedMzx());
14507 assertNull(calibrator.getEstimatedMzy());
14508 assertNull(calibrator.getEstimatedCovariance());
14509 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
14510 assertNotNull(calibrator.getGroundTruthGravityNorm());
14511 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14512 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14513 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
14514 ABSOLUTE_ERROR));
14515 final var g = new Acceleration(0.0, AccelerationUnit.G);
14516 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14517 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14518 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
14519
14520
14521 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
14522 -gravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
14523 }
14524
14525 @Test
14526 void testConstructor130() throws WrongSizeException {
14527 final var ba = generateBa();
14528 final var biasX = ba.getElementAtIndex(0);
14529 final var biasY = ba.getElementAtIndex(1);
14530 final var biasZ = ba.getElementAtIndex(2);
14531
14532 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14533 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14534 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14535
14536 final var ma = generateMaCommonAxis();
14537 final var sx = ma.getElementAt(0, 0);
14538 final var sy = ma.getElementAt(1, 1);
14539 final var sz = ma.getElementAt(2, 2);
14540 final var mxy = ma.getElementAt(0, 1);
14541 final var mxz = ma.getElementAt(0, 2);
14542 final var myx = ma.getElementAt(1, 0);
14543 final var myz = ma.getElementAt(1, 2);
14544 final var mzx = ma.getElementAt(2, 0);
14545 final var mzy = ma.getElementAt(2, 1);
14546
14547 final var randomizer = new UniformRandomizer();
14548 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14549 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14550 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14551 final var nedPosition = new NEDPosition(latitude, longitude, height);
14552 final var nedVelocity = new NEDVelocity();
14553 final var ecefPosition = new ECEFPosition();
14554 final var ecefVelocity = new ECEFVelocity();
14555 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14556 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14557 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14558 final var gravityNorm = gravity.getNorm();
14559
14560 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
14561 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
14562
14563
14564 assertEquals(biasX, calibrator.getBiasX(), 0.0);
14565 assertEquals(biasY, calibrator.getBiasY(), 0.0);
14566 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14567 final var bx1 = calibrator.getBiasXAsAcceleration();
14568 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14569 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14570 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14571 calibrator.getBiasXAsAcceleration(bx2);
14572 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14573 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14574 final var by1 = calibrator.getBiasYAsAcceleration();
14575 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14576 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14577 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14578 calibrator.getBiasYAsAcceleration(by2);
14579 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14580 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14581 final var bz1 = calibrator.getBiasZAsAcceleration();
14582 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14583 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14584 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14585 calibrator.getBiasZAsAcceleration(bz2);
14586 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14587 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14588 final var biasTriad1 = calibrator.getBiasAsTriad();
14589 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14590 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14591 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14592 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14593 final var biasTriad2 = new AccelerationTriad();
14594 calibrator.getBiasAsTriad(biasTriad2);
14595 assertEquals(biasTriad1, biasTriad2);
14596 assertEquals(sx, calibrator.getInitialSx(), 0.0);
14597 assertEquals(sy, calibrator.getInitialSy(), 0.0);
14598 assertEquals(sz, calibrator.getInitialSz(), 0.0);
14599 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
14600 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
14601 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
14602 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
14603 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
14604 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
14605 final var bias1 = calibrator.getBias();
14606 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14607 final var bias2 = new double[3];
14608 calibrator.getBias(bias2);
14609 assertArrayEquals(bias1, bias2, 0.0);
14610 final var b1 = calibrator.getBiasAsMatrix();
14611 assertEquals(b1, ba);
14612 final var b2 = new Matrix(3, 1);
14613 calibrator.getBiasAsMatrix(b2);
14614 assertEquals(b1, b2);
14615 final var ma1 = new Matrix(3, 3);
14616 ma1.setSubmatrix(0, 0, 2, 2,
14617 new double[]{sx, myx, mzx,
14618 mxy, sy, mzy,
14619 mxz, myz, sz});
14620 assertEquals(ma1, calibrator.getInitialMa());
14621 final var ma2 = new Matrix(3, 3);
14622 calibrator.getInitialMa(ma2);
14623 assertEquals(ma1, ma2);
14624 assertNull(calibrator.getMeasurements());
14625 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
14626 calibrator.getMeasurementType());
14627 assertFalse(calibrator.isOrderedMeasurementsRequired());
14628 assertFalse(calibrator.isQualityScoresRequired());
14629 assertTrue(calibrator.isCommonAxisUsed());
14630 assertSame(this, calibrator.getListener());
14631 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
14632 assertFalse(calibrator.isReady());
14633 assertFalse(calibrator.isRunning());
14634 assertNull(calibrator.getEstimatedMa());
14635 assertNull(calibrator.getEstimatedSx());
14636 assertNull(calibrator.getEstimatedSy());
14637 assertNull(calibrator.getEstimatedSz());
14638 assertNull(calibrator.getEstimatedMxy());
14639 assertNull(calibrator.getEstimatedMxz());
14640 assertNull(calibrator.getEstimatedMyx());
14641 assertNull(calibrator.getEstimatedMyz());
14642 assertNull(calibrator.getEstimatedMzx());
14643 assertNull(calibrator.getEstimatedMzy());
14644 assertNull(calibrator.getEstimatedCovariance());
14645 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
14646 assertNotNull(calibrator.getGroundTruthGravityNorm());
14647 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14648 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14649 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
14650 ABSOLUTE_ERROR));
14651 final var g = new Acceleration(0.0, AccelerationUnit.G);
14652 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14653 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14654 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
14655
14656
14657 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
14658 -gravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
14659 }
14660
14661 @Test
14662 void testConstructor131() throws WrongSizeException {
14663 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
14664
14665 final var ba = generateBa();
14666 final var biasX = ba.getElementAtIndex(0);
14667 final var biasY = ba.getElementAtIndex(1);
14668 final var biasZ = ba.getElementAtIndex(2);
14669
14670 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14671 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14672 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14673
14674 final var ma = generateMaCommonAxis();
14675 final var sx = ma.getElementAt(0, 0);
14676 final var sy = ma.getElementAt(1, 1);
14677 final var sz = ma.getElementAt(2, 2);
14678 final var mxy = ma.getElementAt(0, 1);
14679 final var mxz = ma.getElementAt(0, 2);
14680 final var myx = ma.getElementAt(1, 0);
14681 final var myz = ma.getElementAt(1, 2);
14682 final var mzx = ma.getElementAt(2, 0);
14683 final var mzy = ma.getElementAt(2, 1);
14684
14685 final var randomizer = new UniformRandomizer();
14686 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14687 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14688 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14689 final var nedPosition = new NEDPosition(latitude, longitude, height);
14690 final var nedVelocity = new NEDVelocity();
14691 final var ecefPosition = new ECEFPosition();
14692 final var ecefVelocity = new ECEFVelocity();
14693 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14694 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14695 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14696 final var gravityNorm = gravity.getNorm();
14697
14698 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
14699 true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
14700
14701
14702 assertEquals(biasX, calibrator.getBiasX(), 0.0);
14703 assertEquals(biasY, calibrator.getBiasY(), 0.0);
14704 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14705 final var bx1 = calibrator.getBiasXAsAcceleration();
14706 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14707 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14708 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14709 calibrator.getBiasXAsAcceleration(bx2);
14710 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14711 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14712 final var by1 = calibrator.getBiasYAsAcceleration();
14713 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14714 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14715 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14716 calibrator.getBiasYAsAcceleration(by2);
14717 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14718 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14719 final var bz1 = calibrator.getBiasZAsAcceleration();
14720 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14721 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14722 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14723 calibrator.getBiasZAsAcceleration(bz2);
14724 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14725 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14726 final var biasTriad1 = calibrator.getBiasAsTriad();
14727 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14728 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14729 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14730 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14731 final var biasTriad2 = new AccelerationTriad();
14732 calibrator.getBiasAsTriad(biasTriad2);
14733 assertEquals(biasTriad1, biasTriad2);
14734 assertEquals(sx, calibrator.getInitialSx(), 0.0);
14735 assertEquals(sy, calibrator.getInitialSy(), 0.0);
14736 assertEquals(sz, calibrator.getInitialSz(), 0.0);
14737 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
14738 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
14739 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
14740 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
14741 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
14742 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
14743 final var bias1 = calibrator.getBias();
14744 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14745 final var bias2 = new double[3];
14746 calibrator.getBias(bias2);
14747 assertArrayEquals(bias1, bias2, 0.0);
14748 final var b1 = calibrator.getBiasAsMatrix();
14749 assertEquals(b1, ba);
14750 final var b2 = new Matrix(3, 1);
14751 calibrator.getBiasAsMatrix(b2);
14752 assertEquals(b1, b2);
14753 final var ma1 = new Matrix(3, 3);
14754 ma1.setSubmatrix(0, 0, 2, 2,
14755 new double[]{sx, myx, mzx,
14756 mxy, sy, mzy,
14757 mxz, myz, sz});
14758 assertEquals(ma1, calibrator.getInitialMa());
14759 final var ma2 = new Matrix(3, 3);
14760 calibrator.getInitialMa(ma2);
14761 assertEquals(ma1, ma2);
14762 assertSame(measurements, calibrator.getMeasurements());
14763 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
14764 calibrator.getMeasurementType());
14765 assertFalse(calibrator.isOrderedMeasurementsRequired());
14766 assertFalse(calibrator.isQualityScoresRequired());
14767 assertTrue(calibrator.isCommonAxisUsed());
14768 assertNull(calibrator.getListener());
14769 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
14770 assertFalse(calibrator.isReady());
14771 assertFalse(calibrator.isRunning());
14772 assertNull(calibrator.getEstimatedMa());
14773 assertNull(calibrator.getEstimatedSx());
14774 assertNull(calibrator.getEstimatedSy());
14775 assertNull(calibrator.getEstimatedSz());
14776 assertNull(calibrator.getEstimatedMxy());
14777 assertNull(calibrator.getEstimatedMxz());
14778 assertNull(calibrator.getEstimatedMyx());
14779 assertNull(calibrator.getEstimatedMyz());
14780 assertNull(calibrator.getEstimatedMzx());
14781 assertNull(calibrator.getEstimatedMzy());
14782 assertNull(calibrator.getEstimatedCovariance());
14783 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
14784 assertNotNull(calibrator.getGroundTruthGravityNorm());
14785 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14786 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14787 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
14788 ABSOLUTE_ERROR));
14789 final var g = new Acceleration(0.0, AccelerationUnit.G);
14790 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14791 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14792 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
14793
14794
14795 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
14796 -gravityNorm, measurements, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
14797 }
14798
14799 @Test
14800 void testConstructor132() throws WrongSizeException {
14801 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
14802
14803 final var ba = generateBa();
14804 final var biasX = ba.getElementAtIndex(0);
14805 final var biasY = ba.getElementAtIndex(1);
14806 final var biasZ = ba.getElementAtIndex(2);
14807
14808 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14809 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14810 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14811
14812 final var ma = generateMaCommonAxis();
14813 final var sx = ma.getElementAt(0, 0);
14814 final var sy = ma.getElementAt(1, 1);
14815 final var sz = ma.getElementAt(2, 2);
14816 final var mxy = ma.getElementAt(0, 1);
14817 final var mxz = ma.getElementAt(0, 2);
14818 final var myx = ma.getElementAt(1, 0);
14819 final var myz = ma.getElementAt(1, 2);
14820 final var mzx = ma.getElementAt(2, 0);
14821 final var mzy = ma.getElementAt(2, 1);
14822
14823 final var randomizer = new UniformRandomizer();
14824 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14825 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14826 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14827 final var nedPosition = new NEDPosition(latitude, longitude, height);
14828 final var nedVelocity = new NEDVelocity();
14829 final var ecefPosition = new ECEFPosition();
14830 final var ecefVelocity = new ECEFVelocity();
14831 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14832 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14833 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14834 final var gravityNorm = gravity.getNorm();
14835
14836 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
14837 true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
14838
14839
14840 assertEquals(biasX, calibrator.getBiasX(), 0.0);
14841 assertEquals(biasY, calibrator.getBiasY(), 0.0);
14842 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14843 final var bx1 = calibrator.getBiasXAsAcceleration();
14844 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14845 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14846 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14847 calibrator.getBiasXAsAcceleration(bx2);
14848 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14849 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14850 final var by1 = calibrator.getBiasYAsAcceleration();
14851 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14852 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14853 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14854 calibrator.getBiasYAsAcceleration(by2);
14855 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14856 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14857 final var bz1 = calibrator.getBiasZAsAcceleration();
14858 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14859 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14860 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14861 calibrator.getBiasZAsAcceleration(bz2);
14862 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14863 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14864 final var biasTriad1 = calibrator.getBiasAsTriad();
14865 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14866 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14867 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14868 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14869 final var biasTriad2 = new AccelerationTriad();
14870 calibrator.getBiasAsTriad(biasTriad2);
14871 assertEquals(biasTriad1, biasTriad2);
14872 assertEquals(sx, calibrator.getInitialSx(), 0.0);
14873 assertEquals(sy, calibrator.getInitialSy(), 0.0);
14874 assertEquals(sz, calibrator.getInitialSz(), 0.0);
14875 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
14876 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
14877 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
14878 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
14879 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
14880 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
14881 final var bias1 = calibrator.getBias();
14882 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14883 final var bias2 = new double[3];
14884 calibrator.getBias(bias2);
14885 assertArrayEquals(bias1, bias2, 0.0);
14886 final var b1 = calibrator.getBiasAsMatrix();
14887 assertEquals(b1, ba);
14888 final var b2 = new Matrix(3, 1);
14889 calibrator.getBiasAsMatrix(b2);
14890 assertEquals(b1, b2);
14891 final var ma1 = new Matrix(3, 3);
14892 ma1.setSubmatrix(0, 0, 2, 2,
14893 new double[]{sx, myx, mzx,
14894 mxy, sy, mzy,
14895 mxz, myz, sz});
14896 assertEquals(ma1, calibrator.getInitialMa());
14897 final var ma2 = new Matrix(3, 3);
14898 calibrator.getInitialMa(ma2);
14899 assertEquals(ma1, ma2);
14900 assertSame(measurements, calibrator.getMeasurements());
14901 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
14902 calibrator.getMeasurementType());
14903 assertFalse(calibrator.isOrderedMeasurementsRequired());
14904 assertFalse(calibrator.isQualityScoresRequired());
14905 assertTrue(calibrator.isCommonAxisUsed());
14906 assertSame(this, calibrator.getListener());
14907 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
14908 assertFalse(calibrator.isReady());
14909 assertFalse(calibrator.isRunning());
14910 assertNull(calibrator.getEstimatedMa());
14911 assertNull(calibrator.getEstimatedSx());
14912 assertNull(calibrator.getEstimatedSy());
14913 assertNull(calibrator.getEstimatedSz());
14914 assertNull(calibrator.getEstimatedMxy());
14915 assertNull(calibrator.getEstimatedMxz());
14916 assertNull(calibrator.getEstimatedMyx());
14917 assertNull(calibrator.getEstimatedMyz());
14918 assertNull(calibrator.getEstimatedMzx());
14919 assertNull(calibrator.getEstimatedMzy());
14920 assertNull(calibrator.getEstimatedCovariance());
14921 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
14922 assertNotNull(calibrator.getGroundTruthGravityNorm());
14923 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14924 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14925 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
14926 ABSOLUTE_ERROR));
14927 final var g = new Acceleration(0.0, AccelerationUnit.G);
14928 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14929 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14930 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
14931
14932
14933 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
14934 -gravityNorm, measurements, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
14935 }
14936
14937 @Test
14938 void testConstructor133() throws WrongSizeException {
14939 final var ba = generateBa();
14940 final var bias = ba.getBuffer();
14941 final var biasX = ba.getElementAtIndex(0);
14942 final var biasY = ba.getElementAtIndex(1);
14943 final var biasZ = ba.getElementAtIndex(2);
14944
14945 final var randomizer = new UniformRandomizer();
14946 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14947 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14948 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14949 final var nedPosition = new NEDPosition(latitude, longitude, height);
14950 final var nedVelocity = new NEDVelocity();
14951 final var ecefPosition = new ECEFPosition();
14952 final var ecefVelocity = new ECEFVelocity();
14953 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14954 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14955 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14956 final var gravityNorm = gravity.getNorm();
14957
14958 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bias);
14959
14960
14961 assertEquals(biasX, calibrator.getBiasX(), 0.0);
14962 assertEquals(biasY, calibrator.getBiasY(), 0.0);
14963 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14964 final var bx1 = calibrator.getBiasXAsAcceleration();
14965 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14966 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14967 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14968 calibrator.getBiasXAsAcceleration(bx2);
14969 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14970 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14971 final var by1 = calibrator.getBiasYAsAcceleration();
14972 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14973 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14974 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14975 calibrator.getBiasYAsAcceleration(by2);
14976 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14977 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14978 final var bz1 = calibrator.getBiasZAsAcceleration();
14979 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14980 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14981 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14982 calibrator.getBiasZAsAcceleration(bz2);
14983 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14984 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14985 final var biasTriad1 = calibrator.getBiasAsTriad();
14986 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14987 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14988 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14989 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14990 final var biasTriad2 = new AccelerationTriad();
14991 calibrator.getBiasAsTriad(biasTriad2);
14992 assertEquals(biasTriad1, biasTriad2);
14993 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
14994 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
14995 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
14996 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
14997 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
14998 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
14999 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15000 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15001 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15002 final var bias1 = calibrator.getBias();
15003 assertArrayEquals(bias1, bias, 0.0);
15004 final var bias2 = new double[3];
15005 calibrator.getBias(bias2);
15006 assertArrayEquals(bias1, bias2, 0.0);
15007 final var b1 = calibrator.getBiasAsMatrix();
15008 assertEquals(b1, ba);
15009 final var b2 = new Matrix(3, 1);
15010 calibrator.getBiasAsMatrix(b2);
15011 assertEquals(b1, b2);
15012 final var ma1 = calibrator.getInitialMa();
15013 assertEquals(new Matrix(3, 3), ma1);
15014 final var ma2 = new Matrix(3, 3);
15015 calibrator.getInitialMa(ma2);
15016 assertEquals(ma1, ma2);
15017 assertNull(calibrator.getMeasurements());
15018 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15019 calibrator.getMeasurementType());
15020 assertFalse(calibrator.isOrderedMeasurementsRequired());
15021 assertFalse(calibrator.isQualityScoresRequired());
15022 assertFalse(calibrator.isCommonAxisUsed());
15023 assertNull(calibrator.getListener());
15024 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
15025 assertFalse(calibrator.isReady());
15026 assertFalse(calibrator.isRunning());
15027 assertNull(calibrator.getEstimatedMa());
15028 assertNull(calibrator.getEstimatedSx());
15029 assertNull(calibrator.getEstimatedSy());
15030 assertNull(calibrator.getEstimatedSz());
15031 assertNull(calibrator.getEstimatedMxy());
15032 assertNull(calibrator.getEstimatedMxz());
15033 assertNull(calibrator.getEstimatedMyx());
15034 assertNull(calibrator.getEstimatedMyz());
15035 assertNull(calibrator.getEstimatedMzx());
15036 assertNull(calibrator.getEstimatedMzy());
15037 assertNull(calibrator.getEstimatedCovariance());
15038 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15039 assertNotNull(calibrator.getGroundTruthGravityNorm());
15040 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15041 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15042 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15043 ABSOLUTE_ERROR));
15044 final var g = new Acceleration(0.0, AccelerationUnit.G);
15045 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15046 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15047 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15048
15049
15050 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15051 -gravityNorm, bias));
15052 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15053 gravityNorm, new double[1]));
15054 }
15055
15056 @Test
15057 void testConstructor134() throws WrongSizeException {
15058 final var ba = generateBa();
15059 final var bias = ba.getBuffer();
15060 final var biasX = ba.getElementAtIndex(0);
15061 final var biasY = ba.getElementAtIndex(1);
15062 final var biasZ = ba.getElementAtIndex(2);
15063
15064 final var randomizer = new UniformRandomizer();
15065 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15066 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15067 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15068 final var nedPosition = new NEDPosition(latitude, longitude, height);
15069 final var nedVelocity = new NEDVelocity();
15070 final var ecefPosition = new ECEFPosition();
15071 final var ecefVelocity = new ECEFVelocity();
15072 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15073 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15074 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15075 final var gravityNorm = gravity.getNorm();
15076
15077 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bias, this);
15078
15079
15080 assertEquals(biasX, calibrator.getBiasX(), 0.0);
15081 assertEquals(biasY, calibrator.getBiasY(), 0.0);
15082 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15083 final var bx1 = calibrator.getBiasXAsAcceleration();
15084 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15085 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15086 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15087 calibrator.getBiasXAsAcceleration(bx2);
15088 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15089 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15090 final var by1 = calibrator.getBiasYAsAcceleration();
15091 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15092 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15093 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15094 calibrator.getBiasYAsAcceleration(by2);
15095 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15096 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15097 final var bz1 = calibrator.getBiasZAsAcceleration();
15098 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15099 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15100 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15101 calibrator.getBiasZAsAcceleration(bz2);
15102 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15103 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15104 final var biasTriad1 = calibrator.getBiasAsTriad();
15105 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15106 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15107 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15108 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15109 final var biasTriad2 = new AccelerationTriad();
15110 calibrator.getBiasAsTriad(biasTriad2);
15111 assertEquals(biasTriad1, biasTriad2);
15112 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15113 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15114 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15115 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15116 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15117 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15118 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15119 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15120 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15121 final var bias1 = calibrator.getBias();
15122 assertArrayEquals(bias1, bias, 0.0);
15123 final var bias2 = new double[3];
15124 calibrator.getBias(bias2);
15125 assertArrayEquals(bias1, bias2, 0.0);
15126 final var b1 = calibrator.getBiasAsMatrix();
15127 assertEquals(b1, ba);
15128 final var b2 = new Matrix(3, 1);
15129 calibrator.getBiasAsMatrix(b2);
15130 assertEquals(b1, b2);
15131 final var ma1 = calibrator.getInitialMa();
15132 assertEquals(new Matrix(3, 3), ma1);
15133 final var ma2 = new Matrix(3, 3);
15134 calibrator.getInitialMa(ma2);
15135 assertEquals(ma1, ma2);
15136 assertNull(calibrator.getMeasurements());
15137 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15138 calibrator.getMeasurementType());
15139 assertFalse(calibrator.isOrderedMeasurementsRequired());
15140 assertFalse(calibrator.isQualityScoresRequired());
15141 assertFalse(calibrator.isCommonAxisUsed());
15142 assertSame(this, calibrator.getListener());
15143 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
15144 assertFalse(calibrator.isReady());
15145 assertFalse(calibrator.isRunning());
15146 assertNull(calibrator.getEstimatedMa());
15147 assertNull(calibrator.getEstimatedSx());
15148 assertNull(calibrator.getEstimatedSy());
15149 assertNull(calibrator.getEstimatedSz());
15150 assertNull(calibrator.getEstimatedMxy());
15151 assertNull(calibrator.getEstimatedMxz());
15152 assertNull(calibrator.getEstimatedMyx());
15153 assertNull(calibrator.getEstimatedMyz());
15154 assertNull(calibrator.getEstimatedMzx());
15155 assertNull(calibrator.getEstimatedMzy());
15156 assertNull(calibrator.getEstimatedCovariance());
15157 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15158 assertNotNull(calibrator.getGroundTruthGravityNorm());
15159 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15160 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15161 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15162 ABSOLUTE_ERROR));
15163 final var g = new Acceleration(0.0, AccelerationUnit.G);
15164 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15165 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15166 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15167
15168
15169 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15170 -gravityNorm, bias, this));
15171 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15172 gravityNorm, new double[1], this));
15173 }
15174
15175 @Test
15176 void testConstructor135() throws WrongSizeException {
15177 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
15178
15179 final var ba = generateBa();
15180 final var bias = ba.getBuffer();
15181 final var biasX = ba.getElementAtIndex(0);
15182 final var biasY = ba.getElementAtIndex(1);
15183 final var biasZ = ba.getElementAtIndex(2);
15184
15185 final var randomizer = new UniformRandomizer();
15186 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15187 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15188 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15189 final var nedPosition = new NEDPosition(latitude, longitude, height);
15190 final var nedVelocity = new NEDVelocity();
15191 final var ecefPosition = new ECEFPosition();
15192 final var ecefVelocity = new ECEFVelocity();
15193 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15194 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15195 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15196 final var gravityNorm = gravity.getNorm();
15197
15198 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bias);
15199
15200
15201 assertEquals(biasX, calibrator.getBiasX(), 0.0);
15202 assertEquals(biasY, calibrator.getBiasY(), 0.0);
15203 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15204 final var bx1 = calibrator.getBiasXAsAcceleration();
15205 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15206 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15207 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15208 calibrator.getBiasXAsAcceleration(bx2);
15209 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15210 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15211 final var by1 = calibrator.getBiasYAsAcceleration();
15212 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15213 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15214 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15215 calibrator.getBiasYAsAcceleration(by2);
15216 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15217 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15218 final var bz1 = calibrator.getBiasZAsAcceleration();
15219 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15220 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15221 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15222 calibrator.getBiasZAsAcceleration(bz2);
15223 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15224 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15225 final var biasTriad1 = calibrator.getBiasAsTriad();
15226 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15227 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15228 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15229 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15230 final var biasTriad2 = new AccelerationTriad();
15231 calibrator.getBiasAsTriad(biasTriad2);
15232 assertEquals(biasTriad1, biasTriad2);
15233 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15234 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15235 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15236 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15237 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15238 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15239 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15240 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15241 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15242 final var bias1 = calibrator.getBias();
15243 assertArrayEquals(bias1, bias, 0.0);
15244 final var bias2 = new double[3];
15245 calibrator.getBias(bias2);
15246 assertArrayEquals(bias1, bias2, 0.0);
15247 final var b1 = calibrator.getBiasAsMatrix();
15248 assertEquals(b1, ba);
15249 final var b2 = new Matrix(3, 1);
15250 calibrator.getBiasAsMatrix(b2);
15251 assertEquals(b1, b2);
15252 final var ma1 = calibrator.getInitialMa();
15253 assertEquals(new Matrix(3, 3), ma1);
15254 final var ma2 = new Matrix(3, 3);
15255 calibrator.getInitialMa(ma2);
15256 assertEquals(ma1, ma2);
15257 assertSame(measurements, calibrator.getMeasurements());
15258 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15259 calibrator.getMeasurementType());
15260 assertFalse(calibrator.isOrderedMeasurementsRequired());
15261 assertFalse(calibrator.isQualityScoresRequired());
15262 assertFalse(calibrator.isCommonAxisUsed());
15263 assertNull(calibrator.getListener());
15264 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
15265 assertFalse(calibrator.isReady());
15266 assertFalse(calibrator.isRunning());
15267 assertNull(calibrator.getEstimatedMa());
15268 assertNull(calibrator.getEstimatedSx());
15269 assertNull(calibrator.getEstimatedSy());
15270 assertNull(calibrator.getEstimatedSz());
15271 assertNull(calibrator.getEstimatedMxy());
15272 assertNull(calibrator.getEstimatedMxz());
15273 assertNull(calibrator.getEstimatedMyx());
15274 assertNull(calibrator.getEstimatedMyz());
15275 assertNull(calibrator.getEstimatedMzx());
15276 assertNull(calibrator.getEstimatedMzy());
15277 assertNull(calibrator.getEstimatedCovariance());
15278 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15279 assertNotNull(calibrator.getGroundTruthGravityNorm());
15280 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15281 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15282 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15283 ABSOLUTE_ERROR));
15284 final var g = new Acceleration(0.0, AccelerationUnit.G);
15285 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15286 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15287 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15288
15289
15290 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15291 -gravityNorm, measurements, bias));
15292 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15293 gravityNorm, measurements, new double[1]));
15294 }
15295
15296 @Test
15297 void testConstructor136() throws WrongSizeException {
15298 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
15299
15300 final var ba = generateBa();
15301 final var bias = ba.getBuffer();
15302 final var biasX = ba.getElementAtIndex(0);
15303 final var biasY = ba.getElementAtIndex(1);
15304 final var biasZ = ba.getElementAtIndex(2);
15305
15306 final var randomizer = new UniformRandomizer();
15307 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15308 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15309 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15310 final var nedPosition = new NEDPosition(latitude, longitude, height);
15311 final var nedVelocity = new NEDVelocity();
15312 final var ecefPosition = new ECEFPosition();
15313 final var ecefVelocity = new ECEFVelocity();
15314 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15315 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15316 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15317 final var gravityNorm = gravity.getNorm();
15318
15319 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bias,
15320 this);
15321
15322
15323 assertEquals(biasX, calibrator.getBiasX(), 0.0);
15324 assertEquals(biasY, calibrator.getBiasY(), 0.0);
15325 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15326 final var bx1 = calibrator.getBiasXAsAcceleration();
15327 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15328 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15329 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15330 calibrator.getBiasXAsAcceleration(bx2);
15331 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15332 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15333 final var by1 = calibrator.getBiasYAsAcceleration();
15334 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15335 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15336 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15337 calibrator.getBiasYAsAcceleration(by2);
15338 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15339 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15340 final var bz1 = calibrator.getBiasZAsAcceleration();
15341 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15342 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15343 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15344 calibrator.getBiasZAsAcceleration(bz2);
15345 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15346 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15347 final var biasTriad1 = calibrator.getBiasAsTriad();
15348 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15349 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15350 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15351 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15352 final var biasTriad2 = new AccelerationTriad();
15353 calibrator.getBiasAsTriad(biasTriad2);
15354 assertEquals(biasTriad1, biasTriad2);
15355 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15356 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15357 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15358 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15359 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15360 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15361 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15362 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15363 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15364 final var bias1 = calibrator.getBias();
15365 assertArrayEquals(bias1, bias, 0.0);
15366 final var bias2 = new double[3];
15367 calibrator.getBias(bias2);
15368 assertArrayEquals(bias1, bias2, 0.0);
15369 final var b1 = calibrator.getBiasAsMatrix();
15370 assertEquals(b1, ba);
15371 final var b2 = new Matrix(3, 1);
15372 calibrator.getBiasAsMatrix(b2);
15373 assertEquals(b1, b2);
15374 final var ma1 = calibrator.getInitialMa();
15375 assertEquals(new Matrix(3, 3), ma1);
15376 final var ma2 = new Matrix(3, 3);
15377 calibrator.getInitialMa(ma2);
15378 assertEquals(ma1, ma2);
15379 assertSame(measurements, calibrator.getMeasurements());
15380 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15381 calibrator.getMeasurementType());
15382 assertFalse(calibrator.isOrderedMeasurementsRequired());
15383 assertFalse(calibrator.isQualityScoresRequired());
15384 assertFalse(calibrator.isCommonAxisUsed());
15385 assertSame(this, calibrator.getListener());
15386 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
15387 assertFalse(calibrator.isReady());
15388 assertFalse(calibrator.isRunning());
15389 assertNull(calibrator.getEstimatedMa());
15390 assertNull(calibrator.getEstimatedSx());
15391 assertNull(calibrator.getEstimatedSy());
15392 assertNull(calibrator.getEstimatedSz());
15393 assertNull(calibrator.getEstimatedMxy());
15394 assertNull(calibrator.getEstimatedMxz());
15395 assertNull(calibrator.getEstimatedMyx());
15396 assertNull(calibrator.getEstimatedMyz());
15397 assertNull(calibrator.getEstimatedMzx());
15398 assertNull(calibrator.getEstimatedMzy());
15399 assertNull(calibrator.getEstimatedCovariance());
15400 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15401 assertNotNull(calibrator.getGroundTruthGravityNorm());
15402 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15403 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15404 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15405 ABSOLUTE_ERROR));
15406 final var g = new Acceleration(0.0, AccelerationUnit.G);
15407 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15408 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15409 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15410
15411
15412 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15413 -gravityNorm, measurements, bias, this));
15414 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15415 gravityNorm, measurements, new double[1], this));
15416 }
15417
15418 @Test
15419 void testConstructor137() throws WrongSizeException {
15420 final var ba = generateBa();
15421 final var bias = ba.getBuffer();
15422 final var biasX = ba.getElementAtIndex(0);
15423 final var biasY = ba.getElementAtIndex(1);
15424 final var biasZ = ba.getElementAtIndex(2);
15425
15426 final var randomizer = new UniformRandomizer();
15427 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15428 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15429 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15430 final var nedPosition = new NEDPosition(latitude, longitude, height);
15431 final var nedVelocity = new NEDVelocity();
15432 final var ecefPosition = new ECEFPosition();
15433 final var ecefVelocity = new ECEFVelocity();
15434 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15435 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15436 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15437 final var gravityNorm = gravity.getNorm();
15438
15439 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
15440 bias);
15441
15442
15443 assertEquals(biasX, calibrator.getBiasX(), 0.0);
15444 assertEquals(biasY, calibrator.getBiasY(), 0.0);
15445 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15446 final var bx1 = calibrator.getBiasXAsAcceleration();
15447 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15448 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15449 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15450 calibrator.getBiasXAsAcceleration(bx2);
15451 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15452 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15453 final var by1 = calibrator.getBiasYAsAcceleration();
15454 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15455 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15456 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15457 calibrator.getBiasYAsAcceleration(by2);
15458 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15459 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15460 final var bz1 = calibrator.getBiasZAsAcceleration();
15461 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15462 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15463 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15464 calibrator.getBiasZAsAcceleration(bz2);
15465 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15466 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15467 final var biasTriad1 = calibrator.getBiasAsTriad();
15468 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15469 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15470 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15471 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15472 final var biasTriad2 = new AccelerationTriad();
15473 calibrator.getBiasAsTriad(biasTriad2);
15474 assertEquals(biasTriad1, biasTriad2);
15475 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15476 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15477 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15478 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15479 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15480 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15481 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15482 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15483 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15484 final var bias1 = calibrator.getBias();
15485 assertArrayEquals(bias1, bias, 0.0);
15486 final var bias2 = new double[3];
15487 calibrator.getBias(bias2);
15488 assertArrayEquals(bias1, bias2, 0.0);
15489 final var b1 = calibrator.getBiasAsMatrix();
15490 assertEquals(b1, ba);
15491 final var b2 = new Matrix(3, 1);
15492 calibrator.getBiasAsMatrix(b2);
15493 assertEquals(b1, b2);
15494 final var ma1 = calibrator.getInitialMa();
15495 assertEquals(new Matrix(3, 3), ma1);
15496 final var ma2 = new Matrix(3, 3);
15497 calibrator.getInitialMa(ma2);
15498 assertEquals(ma1, ma2);
15499 assertNull(calibrator.getMeasurements());
15500 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15501 calibrator.getMeasurementType());
15502 assertFalse(calibrator.isOrderedMeasurementsRequired());
15503 assertFalse(calibrator.isQualityScoresRequired());
15504 assertTrue(calibrator.isCommonAxisUsed());
15505 assertNull(calibrator.getListener());
15506 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
15507 assertFalse(calibrator.isReady());
15508 assertFalse(calibrator.isRunning());
15509 assertNull(calibrator.getEstimatedMa());
15510 assertNull(calibrator.getEstimatedSx());
15511 assertNull(calibrator.getEstimatedSy());
15512 assertNull(calibrator.getEstimatedSz());
15513 assertNull(calibrator.getEstimatedMxy());
15514 assertNull(calibrator.getEstimatedMxz());
15515 assertNull(calibrator.getEstimatedMyx());
15516 assertNull(calibrator.getEstimatedMyz());
15517 assertNull(calibrator.getEstimatedMzx());
15518 assertNull(calibrator.getEstimatedMzy());
15519 assertNull(calibrator.getEstimatedCovariance());
15520 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15521 assertNotNull(calibrator.getGroundTruthGravityNorm());
15522 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15523 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15524 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15525 ABSOLUTE_ERROR));
15526 final var g = new Acceleration(0.0, AccelerationUnit.G);
15527 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15528 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15529 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15530
15531
15532 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15533 -gravityNorm, true, bias));
15534 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15535 gravityNorm, true, new double[1]));
15536 }
15537
15538 @Test
15539 void testConstructor138() throws WrongSizeException {
15540 final var ba = generateBa();
15541 final var bias = ba.getBuffer();
15542 final var biasX = ba.getElementAtIndex(0);
15543 final var biasY = ba.getElementAtIndex(1);
15544 final var biasZ = ba.getElementAtIndex(2);
15545
15546 final var randomizer = new UniformRandomizer();
15547 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15548 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15549 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15550 final var nedPosition = new NEDPosition(latitude, longitude, height);
15551 final var nedVelocity = new NEDVelocity();
15552 final var ecefPosition = new ECEFPosition();
15553 final var ecefVelocity = new ECEFVelocity();
15554 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15555 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15556 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15557 final var gravityNorm = gravity.getNorm();
15558
15559 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
15560 bias, this);
15561
15562
15563 assertEquals(biasX, calibrator.getBiasX(), 0.0);
15564 assertEquals(biasY, calibrator.getBiasY(), 0.0);
15565 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15566 final var bx1 = calibrator.getBiasXAsAcceleration();
15567 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15568 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15569 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15570 calibrator.getBiasXAsAcceleration(bx2);
15571 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15572 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15573 final var by1 = calibrator.getBiasYAsAcceleration();
15574 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15575 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15576 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15577 calibrator.getBiasYAsAcceleration(by2);
15578 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15579 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15580 final var bz1 = calibrator.getBiasZAsAcceleration();
15581 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15582 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15583 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15584 calibrator.getBiasZAsAcceleration(bz2);
15585 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15586 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15587 final var biasTriad1 = calibrator.getBiasAsTriad();
15588 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15589 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15590 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15591 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15592 final var biasTriad2 = new AccelerationTriad();
15593 calibrator.getBiasAsTriad(biasTriad2);
15594 assertEquals(biasTriad1, biasTriad2);
15595 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15596 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15597 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15598 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15599 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15600 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15601 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15602 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15603 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15604 final var bias1 = calibrator.getBias();
15605 assertArrayEquals(bias1, bias, 0.0);
15606 final var bias2 = new double[3];
15607 calibrator.getBias(bias2);
15608 assertArrayEquals(bias1, bias2, 0.0);
15609 final var b1 = calibrator.getBiasAsMatrix();
15610 assertEquals(b1, ba);
15611 final var b2 = new Matrix(3, 1);
15612 calibrator.getBiasAsMatrix(b2);
15613 assertEquals(b1, b2);
15614 final var ma1 = calibrator.getInitialMa();
15615 assertEquals(new Matrix(3, 3), ma1);
15616 final var ma2 = new Matrix(3, 3);
15617 calibrator.getInitialMa(ma2);
15618 assertEquals(ma1, ma2);
15619 assertNull(calibrator.getMeasurements());
15620 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15621 calibrator.getMeasurementType());
15622 assertFalse(calibrator.isOrderedMeasurementsRequired());
15623 assertFalse(calibrator.isQualityScoresRequired());
15624 assertTrue(calibrator.isCommonAxisUsed());
15625 assertSame(this, calibrator.getListener());
15626 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
15627 assertFalse(calibrator.isReady());
15628 assertFalse(calibrator.isRunning());
15629 assertNull(calibrator.getEstimatedMa());
15630 assertNull(calibrator.getEstimatedSx());
15631 assertNull(calibrator.getEstimatedSy());
15632 assertNull(calibrator.getEstimatedSz());
15633 assertNull(calibrator.getEstimatedMxy());
15634 assertNull(calibrator.getEstimatedMxz());
15635 assertNull(calibrator.getEstimatedMyx());
15636 assertNull(calibrator.getEstimatedMyz());
15637 assertNull(calibrator.getEstimatedMzx());
15638 assertNull(calibrator.getEstimatedMzy());
15639 assertNull(calibrator.getEstimatedCovariance());
15640 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15641 assertNotNull(calibrator.getGroundTruthGravityNorm());
15642 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15643 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15644 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15645 ABSOLUTE_ERROR));
15646 final var g = new Acceleration(0.0, AccelerationUnit.G);
15647 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15648 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15649 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15650
15651
15652 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15653 -gravityNorm, true, bias, this));
15654 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15655 gravityNorm, true, new double[1], this));
15656 }
15657
15658 @Test
15659 void testConstructor139() throws WrongSizeException {
15660 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
15661
15662 final var ba = generateBa();
15663 final var bias = ba.getBuffer();
15664 final var biasX = ba.getElementAtIndex(0);
15665 final var biasY = ba.getElementAtIndex(1);
15666 final var biasZ = ba.getElementAtIndex(2);
15667
15668 final var randomizer = new UniformRandomizer();
15669 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15670 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15671 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15672 final var nedPosition = new NEDPosition(latitude, longitude, height);
15673 final var nedVelocity = new NEDVelocity();
15674 final var ecefPosition = new ECEFPosition();
15675 final var ecefVelocity = new ECEFVelocity();
15676 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15677 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15678 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15679 final var gravityNorm = gravity.getNorm();
15680
15681 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
15682 true, bias);
15683
15684
15685 assertEquals(biasX, calibrator.getBiasX(), 0.0);
15686 assertEquals(biasY, calibrator.getBiasY(), 0.0);
15687 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15688 final var bx1 = calibrator.getBiasXAsAcceleration();
15689 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15690 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15691 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15692 calibrator.getBiasXAsAcceleration(bx2);
15693 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15694 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15695 final var by1 = calibrator.getBiasYAsAcceleration();
15696 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15697 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15698 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15699 calibrator.getBiasYAsAcceleration(by2);
15700 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15701 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15702 final var bz1 = calibrator.getBiasZAsAcceleration();
15703 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15704 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15705 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15706 calibrator.getBiasZAsAcceleration(bz2);
15707 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15708 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15709 final var biasTriad1 = calibrator.getBiasAsTriad();
15710 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15711 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15712 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15713 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15714 final var biasTriad2 = new AccelerationTriad();
15715 calibrator.getBiasAsTriad(biasTriad2);
15716 assertEquals(biasTriad1, biasTriad2);
15717 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15718 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15719 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15720 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15721 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15722 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15723 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15724 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15725 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15726 final var bias1 = calibrator.getBias();
15727 assertArrayEquals(bias1, bias, 0.0);
15728 final var bias2 = new double[3];
15729 calibrator.getBias(bias2);
15730 assertArrayEquals(bias1, bias2, 0.0);
15731 final var b1 = calibrator.getBiasAsMatrix();
15732 assertEquals(b1, ba);
15733 final var b2 = new Matrix(3, 1);
15734 calibrator.getBiasAsMatrix(b2);
15735 assertEquals(b1, b2);
15736 final var ma1 = calibrator.getInitialMa();
15737 assertEquals(new Matrix(3, 3), ma1);
15738 final var ma2 = new Matrix(3, 3);
15739 calibrator.getInitialMa(ma2);
15740 assertEquals(ma1, ma2);
15741 assertSame(measurements, calibrator.getMeasurements());
15742 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15743 calibrator.getMeasurementType());
15744 assertFalse(calibrator.isOrderedMeasurementsRequired());
15745 assertFalse(calibrator.isQualityScoresRequired());
15746 assertTrue(calibrator.isCommonAxisUsed());
15747 assertNull(calibrator.getListener());
15748 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
15749 assertFalse(calibrator.isReady());
15750 assertFalse(calibrator.isRunning());
15751 assertNull(calibrator.getEstimatedMa());
15752 assertNull(calibrator.getEstimatedSx());
15753 assertNull(calibrator.getEstimatedSy());
15754 assertNull(calibrator.getEstimatedSz());
15755 assertNull(calibrator.getEstimatedMxy());
15756 assertNull(calibrator.getEstimatedMxz());
15757 assertNull(calibrator.getEstimatedMyx());
15758 assertNull(calibrator.getEstimatedMyz());
15759 assertNull(calibrator.getEstimatedMzx());
15760 assertNull(calibrator.getEstimatedMzy());
15761 assertNull(calibrator.getEstimatedCovariance());
15762 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15763 assertNotNull(calibrator.getGroundTruthGravityNorm());
15764 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15765 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15766 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15767 ABSOLUTE_ERROR));
15768 final var g = new Acceleration(0.0, AccelerationUnit.G);
15769 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15770 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15771 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15772
15773
15774 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15775 -gravityNorm, measurements, true, bias));
15776 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15777 gravityNorm, measurements, true, new double[1]));
15778 }
15779
15780 @Test
15781 void testConstructor140() throws WrongSizeException {
15782 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
15783
15784 final var ba = generateBa();
15785 final var bias = ba.getBuffer();
15786 final var biasX = ba.getElementAtIndex(0);
15787 final var biasY = ba.getElementAtIndex(1);
15788 final var biasZ = ba.getElementAtIndex(2);
15789
15790 final var randomizer = new UniformRandomizer();
15791 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15792 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15793 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15794 final var nedPosition = new NEDPosition(latitude, longitude, height);
15795 final var nedVelocity = new NEDVelocity();
15796 final var ecefPosition = new ECEFPosition();
15797 final var ecefVelocity = new ECEFVelocity();
15798 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15799 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15800 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15801 final var gravityNorm = gravity.getNorm();
15802
15803 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
15804 true, bias, this);
15805
15806
15807 assertEquals(biasX, calibrator.getBiasX(), 0.0);
15808 assertEquals(biasY, calibrator.getBiasY(), 0.0);
15809 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15810 final var bx1 = calibrator.getBiasXAsAcceleration();
15811 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15812 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15813 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15814 calibrator.getBiasXAsAcceleration(bx2);
15815 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15816 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15817 final var by1 = calibrator.getBiasYAsAcceleration();
15818 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15819 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15820 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15821 calibrator.getBiasYAsAcceleration(by2);
15822 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15823 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15824 final var bz1 = calibrator.getBiasZAsAcceleration();
15825 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15826 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15827 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15828 calibrator.getBiasZAsAcceleration(bz2);
15829 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15830 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15831 final var biasTriad1 = calibrator.getBiasAsTriad();
15832 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15833 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15834 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15835 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15836 final var biasTriad2 = new AccelerationTriad();
15837 calibrator.getBiasAsTriad(biasTriad2);
15838 assertEquals(biasTriad1, biasTriad2);
15839 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15840 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15841 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15842 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15843 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15844 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15845 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15846 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15847 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15848 final var bias1 = calibrator.getBias();
15849 assertArrayEquals(bias1, bias, 0.0);
15850 final var bias2 = new double[3];
15851 calibrator.getBias(bias2);
15852 assertArrayEquals(bias1, bias2, 0.0);
15853 final var b1 = calibrator.getBiasAsMatrix();
15854 assertEquals(b1, ba);
15855 final var b2 = new Matrix(3, 1);
15856 calibrator.getBiasAsMatrix(b2);
15857 assertEquals(b1, b2);
15858 final var ma1 = calibrator.getInitialMa();
15859 assertEquals(new Matrix(3, 3), ma1);
15860 final var ma2 = new Matrix(3, 3);
15861 calibrator.getInitialMa(ma2);
15862 assertEquals(ma1, ma2);
15863 assertSame(measurements, calibrator.getMeasurements());
15864 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15865 calibrator.getMeasurementType());
15866 assertFalse(calibrator.isOrderedMeasurementsRequired());
15867 assertFalse(calibrator.isQualityScoresRequired());
15868 assertTrue(calibrator.isCommonAxisUsed());
15869 assertSame(this, calibrator.getListener());
15870 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
15871 assertFalse(calibrator.isReady());
15872 assertFalse(calibrator.isRunning());
15873 assertNull(calibrator.getEstimatedMa());
15874 assertNull(calibrator.getEstimatedSx());
15875 assertNull(calibrator.getEstimatedSy());
15876 assertNull(calibrator.getEstimatedSz());
15877 assertNull(calibrator.getEstimatedMxy());
15878 assertNull(calibrator.getEstimatedMxz());
15879 assertNull(calibrator.getEstimatedMyx());
15880 assertNull(calibrator.getEstimatedMyz());
15881 assertNull(calibrator.getEstimatedMzx());
15882 assertNull(calibrator.getEstimatedMzy());
15883 assertNull(calibrator.getEstimatedCovariance());
15884 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15885 assertNotNull(calibrator.getGroundTruthGravityNorm());
15886 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15887 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15888 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15889 ABSOLUTE_ERROR));
15890 final var g = new Acceleration(0.0, AccelerationUnit.G);
15891 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15892 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15893 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15894
15895
15896 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15897 -gravityNorm, measurements, true, bias, this));
15898 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15899 gravityNorm, measurements, true, new double[1], this));
15900 }
15901
15902 @Test
15903 void testConstructor141() throws WrongSizeException {
15904 final var ba = generateBa();
15905 final var bias = ba.getBuffer();
15906 final var biasX = ba.getElementAtIndex(0);
15907 final var biasY = ba.getElementAtIndex(1);
15908 final var biasZ = ba.getElementAtIndex(2);
15909
15910 final var randomizer = new UniformRandomizer();
15911 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15912 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15913 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15914 final var nedPosition = new NEDPosition(latitude, longitude, height);
15915 final var nedVelocity = new NEDVelocity();
15916 final var ecefPosition = new ECEFPosition();
15917 final var ecefVelocity = new ECEFVelocity();
15918 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15919 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15920 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15921 final var gravityNorm = gravity.getNorm();
15922
15923 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba);
15924
15925
15926 assertEquals(biasX, calibrator.getBiasX(), 0.0);
15927 assertEquals(biasY, calibrator.getBiasY(), 0.0);
15928 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15929 final var bx1 = calibrator.getBiasXAsAcceleration();
15930 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15931 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15932 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15933 calibrator.getBiasXAsAcceleration(bx2);
15934 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15935 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15936 final var by1 = calibrator.getBiasYAsAcceleration();
15937 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15938 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15939 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15940 calibrator.getBiasYAsAcceleration(by2);
15941 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15942 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15943 final var bz1 = calibrator.getBiasZAsAcceleration();
15944 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15945 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15946 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15947 calibrator.getBiasZAsAcceleration(bz2);
15948 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15949 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15950 final var biasTriad1 = calibrator.getBiasAsTriad();
15951 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15952 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15953 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15954 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15955 final var biasTriad2 = new AccelerationTriad();
15956 calibrator.getBiasAsTriad(biasTriad2);
15957 assertEquals(biasTriad1, biasTriad2);
15958 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15959 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15960 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15961 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15962 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15963 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15964 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15965 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15966 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15967 final var bias1 = calibrator.getBias();
15968 assertArrayEquals(bias1, bias, 0.0);
15969 final var bias2 = new double[3];
15970 calibrator.getBias(bias2);
15971 assertArrayEquals(bias1, bias2, 0.0);
15972 final var b1 = calibrator.getBiasAsMatrix();
15973 assertEquals(b1, ba);
15974 final var b2 = new Matrix(3, 1);
15975 calibrator.getBiasAsMatrix(b2);
15976 assertEquals(b1, b2);
15977 final var ma1 = calibrator.getInitialMa();
15978 assertEquals(new Matrix(3, 3), ma1);
15979 final var ma2 = new Matrix(3, 3);
15980 calibrator.getInitialMa(ma2);
15981 assertEquals(ma1, ma2);
15982 assertNull(calibrator.getMeasurements());
15983 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15984 calibrator.getMeasurementType());
15985 assertFalse(calibrator.isOrderedMeasurementsRequired());
15986 assertFalse(calibrator.isQualityScoresRequired());
15987 assertFalse(calibrator.isCommonAxisUsed());
15988 assertNull(calibrator.getListener());
15989 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
15990 assertFalse(calibrator.isReady());
15991 assertFalse(calibrator.isRunning());
15992 assertNull(calibrator.getEstimatedMa());
15993 assertNull(calibrator.getEstimatedSx());
15994 assertNull(calibrator.getEstimatedSy());
15995 assertNull(calibrator.getEstimatedSz());
15996 assertNull(calibrator.getEstimatedMxy());
15997 assertNull(calibrator.getEstimatedMxz());
15998 assertNull(calibrator.getEstimatedMyx());
15999 assertNull(calibrator.getEstimatedMyz());
16000 assertNull(calibrator.getEstimatedMzx());
16001 assertNull(calibrator.getEstimatedMzy());
16002 assertNull(calibrator.getEstimatedCovariance());
16003 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16004 assertNotNull(calibrator.getGroundTruthGravityNorm());
16005 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16006 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16007 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16008 ABSOLUTE_ERROR));
16009 final var g = new Acceleration(0.0, AccelerationUnit.G);
16010 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16011 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16012 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16013
16014
16015 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16016 -gravityNorm, ba));
16017 final var m1 = new Matrix(1, 1);
16018 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16019 gravityNorm, m1));
16020 final var m2 = new Matrix(1, 3);
16021 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16022 gravityNorm, m2));
16023 }
16024
16025 @Test
16026 void testConstructor142() throws WrongSizeException {
16027 final var ba = generateBa();
16028 final var bias = ba.getBuffer();
16029 final var biasX = ba.getElementAtIndex(0);
16030 final var biasY = ba.getElementAtIndex(1);
16031 final var biasZ = ba.getElementAtIndex(2);
16032
16033 final var randomizer = new UniformRandomizer();
16034 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16035 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16036 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16037 final var nedPosition = new NEDPosition(latitude, longitude, height);
16038 final var nedVelocity = new NEDVelocity();
16039 final var ecefPosition = new ECEFPosition();
16040 final var ecefVelocity = new ECEFVelocity();
16041 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16042 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16043 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16044 final var gravityNorm = gravity.getNorm();
16045
16046 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba, this);
16047
16048
16049 assertEquals(biasX, calibrator.getBiasX(), 0.0);
16050 assertEquals(biasY, calibrator.getBiasY(), 0.0);
16051 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16052 final var bx1 = calibrator.getBiasXAsAcceleration();
16053 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16054 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16055 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16056 calibrator.getBiasXAsAcceleration(bx2);
16057 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16058 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16059 final var by1 = calibrator.getBiasYAsAcceleration();
16060 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16061 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16062 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16063 calibrator.getBiasYAsAcceleration(by2);
16064 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16065 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16066 final var bz1 = calibrator.getBiasZAsAcceleration();
16067 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16068 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16069 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16070 calibrator.getBiasZAsAcceleration(bz2);
16071 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16072 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16073 final var biasTriad1 = calibrator.getBiasAsTriad();
16074 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16075 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16076 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16077 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16078 final var biasTriad2 = new AccelerationTriad();
16079 calibrator.getBiasAsTriad(biasTriad2);
16080 assertEquals(biasTriad1, biasTriad2);
16081 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
16082 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
16083 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
16084 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
16085 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
16086 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
16087 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
16088 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
16089 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
16090 final var bias1 = calibrator.getBias();
16091 assertArrayEquals(bias1, bias, 0.0);
16092 final var bias2 = new double[3];
16093 calibrator.getBias(bias2);
16094 assertArrayEquals(bias1, bias2, 0.0);
16095 final var b1 = calibrator.getBiasAsMatrix();
16096 assertEquals(b1, ba);
16097 final var b2 = new Matrix(3, 1);
16098 calibrator.getBiasAsMatrix(b2);
16099 assertEquals(b1, b2);
16100 final var ma1 = calibrator.getInitialMa();
16101 assertEquals(new Matrix(3, 3), ma1);
16102 final var ma2 = new Matrix(3, 3);
16103 calibrator.getInitialMa(ma2);
16104 assertEquals(ma1, ma2);
16105 assertNull(calibrator.getMeasurements());
16106 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16107 calibrator.getMeasurementType());
16108 assertFalse(calibrator.isOrderedMeasurementsRequired());
16109 assertFalse(calibrator.isQualityScoresRequired());
16110 assertFalse(calibrator.isCommonAxisUsed());
16111 assertSame(this, calibrator.getListener());
16112 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
16113 assertFalse(calibrator.isReady());
16114 assertFalse(calibrator.isRunning());
16115 assertNull(calibrator.getEstimatedMa());
16116 assertNull(calibrator.getEstimatedSx());
16117 assertNull(calibrator.getEstimatedSy());
16118 assertNull(calibrator.getEstimatedSz());
16119 assertNull(calibrator.getEstimatedMxy());
16120 assertNull(calibrator.getEstimatedMxz());
16121 assertNull(calibrator.getEstimatedMyx());
16122 assertNull(calibrator.getEstimatedMyz());
16123 assertNull(calibrator.getEstimatedMzx());
16124 assertNull(calibrator.getEstimatedMzy());
16125 assertNull(calibrator.getEstimatedCovariance());
16126 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16127 assertNotNull(calibrator.getGroundTruthGravityNorm());
16128 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16129 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16130 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16131 ABSOLUTE_ERROR));
16132 final var g = new Acceleration(0.0, AccelerationUnit.G);
16133 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16134 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16135 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16136
16137
16138 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16139 -gravityNorm, ba, this));
16140 final var m1 = new Matrix(1, 1);
16141 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16142 gravityNorm, m1, this));
16143 final var m2 = new Matrix(1, 3);
16144 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16145 gravityNorm, m2, this));
16146 }
16147
16148 @Test
16149 void testConstructor143() throws WrongSizeException {
16150 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
16151
16152 final var ba = generateBa();
16153 final var bias = ba.getBuffer();
16154 final var biasX = ba.getElementAtIndex(0);
16155 final var biasY = ba.getElementAtIndex(1);
16156 final var biasZ = ba.getElementAtIndex(2);
16157
16158 final var randomizer = new UniformRandomizer();
16159 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16160 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16161 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16162 final var nedPosition = new NEDPosition(latitude, longitude, height);
16163 final var nedVelocity = new NEDVelocity();
16164 final var ecefPosition = new ECEFPosition();
16165 final var ecefVelocity = new ECEFVelocity();
16166 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16167 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16168 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16169 final var gravityNorm = gravity.getNorm();
16170
16171 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba);
16172
16173
16174 assertEquals(biasX, calibrator.getBiasX(), 0.0);
16175 assertEquals(biasY, calibrator.getBiasY(), 0.0);
16176 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16177 final var bx1 = calibrator.getBiasXAsAcceleration();
16178 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16179 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16180 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16181 calibrator.getBiasXAsAcceleration(bx2);
16182 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16183 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16184 final var by1 = calibrator.getBiasYAsAcceleration();
16185 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16186 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16187 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16188 calibrator.getBiasYAsAcceleration(by2);
16189 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16190 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16191 final var bz1 = calibrator.getBiasZAsAcceleration();
16192 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16193 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16194 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16195 calibrator.getBiasZAsAcceleration(bz2);
16196 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16197 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16198 final var biasTriad1 = calibrator.getBiasAsTriad();
16199 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16200 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16201 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16202 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16203 final var biasTriad2 = new AccelerationTriad();
16204 calibrator.getBiasAsTriad(biasTriad2);
16205 assertEquals(biasTriad1, biasTriad2);
16206 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
16207 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
16208 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
16209 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
16210 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
16211 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
16212 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
16213 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
16214 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
16215 final var bias1 = calibrator.getBias();
16216 assertArrayEquals(bias1, bias, 0.0);
16217 final var bias2 = new double[3];
16218 calibrator.getBias(bias2);
16219 assertArrayEquals(bias1, bias2, 0.0);
16220 final var b1 = calibrator.getBiasAsMatrix();
16221 assertEquals(b1, ba);
16222 final var b2 = new Matrix(3, 1);
16223 calibrator.getBiasAsMatrix(b2);
16224 assertEquals(b1, b2);
16225 final var ma1 = calibrator.getInitialMa();
16226 assertEquals(new Matrix(3, 3), ma1);
16227 final var ma2 = new Matrix(3, 3);
16228 calibrator.getInitialMa(ma2);
16229 assertEquals(ma1, ma2);
16230 assertSame(measurements, calibrator.getMeasurements());
16231 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16232 calibrator.getMeasurementType());
16233 assertFalse(calibrator.isOrderedMeasurementsRequired());
16234 assertFalse(calibrator.isQualityScoresRequired());
16235 assertFalse(calibrator.isCommonAxisUsed());
16236 assertNull(calibrator.getListener());
16237 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
16238 assertFalse(calibrator.isReady());
16239 assertFalse(calibrator.isRunning());
16240 assertNull(calibrator.getEstimatedMa());
16241 assertNull(calibrator.getEstimatedSx());
16242 assertNull(calibrator.getEstimatedSy());
16243 assertNull(calibrator.getEstimatedSz());
16244 assertNull(calibrator.getEstimatedMxy());
16245 assertNull(calibrator.getEstimatedMxz());
16246 assertNull(calibrator.getEstimatedMyx());
16247 assertNull(calibrator.getEstimatedMyz());
16248 assertNull(calibrator.getEstimatedMzx());
16249 assertNull(calibrator.getEstimatedMzy());
16250 assertNull(calibrator.getEstimatedCovariance());
16251 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16252 assertNotNull(calibrator.getGroundTruthGravityNorm());
16253 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16254 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16255 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16256 ABSOLUTE_ERROR));
16257 final var g = new Acceleration(0.0, AccelerationUnit.G);
16258 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16259 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16260 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16261
16262
16263 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16264 -gravityNorm, measurements, ba));
16265 final var m1 = new Matrix(1, 1);
16266 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16267 gravityNorm, measurements, m1));
16268 final var m2 = new Matrix(1, 3);
16269 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16270 gravityNorm, measurements, m2));
16271 }
16272
16273 @Test
16274 void testConstructor144() throws WrongSizeException {
16275 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
16276
16277 final var ba = generateBa();
16278 final var bias = ba.getBuffer();
16279 final var biasX = ba.getElementAtIndex(0);
16280 final var biasY = ba.getElementAtIndex(1);
16281 final var biasZ = ba.getElementAtIndex(2);
16282
16283 final var randomizer = new UniformRandomizer();
16284 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16285 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16286 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16287 final var nedPosition = new NEDPosition(latitude, longitude, height);
16288 final var nedVelocity = new NEDVelocity();
16289 final var ecefPosition = new ECEFPosition();
16290 final var ecefVelocity = new ECEFVelocity();
16291 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16292 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16293 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16294 final var gravityNorm = gravity.getNorm();
16295
16296 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba,
16297 this);
16298
16299
16300 assertEquals(biasX, calibrator.getBiasX(), 0.0);
16301 assertEquals(biasY, calibrator.getBiasY(), 0.0);
16302 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16303 final var bx1 = calibrator.getBiasXAsAcceleration();
16304 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16305 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16306 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16307 calibrator.getBiasXAsAcceleration(bx2);
16308 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16309 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16310 final var by1 = calibrator.getBiasYAsAcceleration();
16311 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16312 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16313 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16314 calibrator.getBiasYAsAcceleration(by2);
16315 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16316 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16317 final var bz1 = calibrator.getBiasZAsAcceleration();
16318 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16319 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16320 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16321 calibrator.getBiasZAsAcceleration(bz2);
16322 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16323 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16324 final var biasTriad1 = calibrator.getBiasAsTriad();
16325 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16326 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16327 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16328 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16329 final var biasTriad2 = new AccelerationTriad();
16330 calibrator.getBiasAsTriad(biasTriad2);
16331 assertEquals(biasTriad1, biasTriad2);
16332 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
16333 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
16334 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
16335 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
16336 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
16337 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
16338 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
16339 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
16340 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
16341 final var bias1 = calibrator.getBias();
16342 assertArrayEquals(bias1, bias, 0.0);
16343 final var bias2 = new double[3];
16344 calibrator.getBias(bias2);
16345 assertArrayEquals(bias1, bias2, 0.0);
16346 final var b1 = calibrator.getBiasAsMatrix();
16347 assertEquals(b1, ba);
16348 final var b2 = new Matrix(3, 1);
16349 calibrator.getBiasAsMatrix(b2);
16350 assertEquals(b1, b2);
16351 final var ma1 = calibrator.getInitialMa();
16352 assertEquals(new Matrix(3, 3), ma1);
16353 final var ma2 = new Matrix(3, 3);
16354 calibrator.getInitialMa(ma2);
16355 assertEquals(ma1, ma2);
16356 assertSame(measurements, calibrator.getMeasurements());
16357 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16358 calibrator.getMeasurementType());
16359 assertFalse(calibrator.isOrderedMeasurementsRequired());
16360 assertFalse(calibrator.isQualityScoresRequired());
16361 assertFalse(calibrator.isCommonAxisUsed());
16362 assertSame(this, calibrator.getListener());
16363 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
16364 assertFalse(calibrator.isReady());
16365 assertFalse(calibrator.isRunning());
16366 assertNull(calibrator.getEstimatedMa());
16367 assertNull(calibrator.getEstimatedSx());
16368 assertNull(calibrator.getEstimatedSy());
16369 assertNull(calibrator.getEstimatedSz());
16370 assertNull(calibrator.getEstimatedMxy());
16371 assertNull(calibrator.getEstimatedMxz());
16372 assertNull(calibrator.getEstimatedMyx());
16373 assertNull(calibrator.getEstimatedMyz());
16374 assertNull(calibrator.getEstimatedMzx());
16375 assertNull(calibrator.getEstimatedMzy());
16376 assertNull(calibrator.getEstimatedCovariance());
16377 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16378 assertNotNull(calibrator.getGroundTruthGravityNorm());
16379 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16380 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16381 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16382 ABSOLUTE_ERROR));
16383 final var g = new Acceleration(0.0, AccelerationUnit.G);
16384 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16385 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16386 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16387
16388
16389 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16390 -gravityNorm, measurements, ba, this));
16391 final var m1 = new Matrix(1, 1);
16392 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16393 gravityNorm, measurements, m1, this));
16394 final var m2 = new Matrix(1, 3);
16395 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16396 gravityNorm, measurements, m2, this));
16397 }
16398
16399 @Test
16400 void testConstructor145() throws WrongSizeException {
16401 final var ba = generateBa();
16402 final var bias = ba.getBuffer();
16403 final var biasX = ba.getElementAtIndex(0);
16404 final var biasY = ba.getElementAtIndex(1);
16405 final var biasZ = ba.getElementAtIndex(2);
16406
16407 final var randomizer = new UniformRandomizer();
16408 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16409 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16410 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16411 final var nedPosition = new NEDPosition(latitude, longitude, height);
16412 final var nedVelocity = new NEDVelocity();
16413 final var ecefPosition = new ECEFPosition();
16414 final var ecefVelocity = new ECEFVelocity();
16415 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16416 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16417 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16418 final var gravityNorm = gravity.getNorm();
16419
16420 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, ba);
16421
16422
16423 assertEquals(biasX, calibrator.getBiasX(), 0.0);
16424 assertEquals(biasY, calibrator.getBiasY(), 0.0);
16425 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16426 final var bx1 = calibrator.getBiasXAsAcceleration();
16427 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16428 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16429 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16430 calibrator.getBiasXAsAcceleration(bx2);
16431 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16432 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16433 final var by1 = calibrator.getBiasYAsAcceleration();
16434 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16435 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16436 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16437 calibrator.getBiasYAsAcceleration(by2);
16438 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16439 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16440 final var bz1 = calibrator.getBiasZAsAcceleration();
16441 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16442 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16443 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16444 calibrator.getBiasZAsAcceleration(bz2);
16445 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16446 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16447 final var biasTriad1 = calibrator.getBiasAsTriad();
16448 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16449 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16450 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16451 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16452 final var biasTriad2 = new AccelerationTriad();
16453 calibrator.getBiasAsTriad(biasTriad2);
16454 assertEquals(biasTriad1, biasTriad2);
16455 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
16456 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
16457 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
16458 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
16459 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
16460 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
16461 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
16462 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
16463 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
16464 final var bias1 = calibrator.getBias();
16465 assertArrayEquals(bias1, bias, 0.0);
16466 final var bias2 = new double[3];
16467 calibrator.getBias(bias2);
16468 assertArrayEquals(bias1, bias2, 0.0);
16469 final var b1 = calibrator.getBiasAsMatrix();
16470 assertEquals(b1, ba);
16471 final var b2 = new Matrix(3, 1);
16472 calibrator.getBiasAsMatrix(b2);
16473 assertEquals(b1, b2);
16474 final var ma1 = calibrator.getInitialMa();
16475 assertEquals(new Matrix(3, 3), ma1);
16476 final var ma2 = new Matrix(3, 3);
16477 calibrator.getInitialMa(ma2);
16478 assertEquals(ma1, ma2);
16479 assertNull(calibrator.getMeasurements());
16480 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16481 calibrator.getMeasurementType());
16482 assertFalse(calibrator.isOrderedMeasurementsRequired());
16483 assertFalse(calibrator.isQualityScoresRequired());
16484 assertTrue(calibrator.isCommonAxisUsed());
16485 assertNull(calibrator.getListener());
16486 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
16487 assertFalse(calibrator.isReady());
16488 assertFalse(calibrator.isRunning());
16489 assertNull(calibrator.getEstimatedMa());
16490 assertNull(calibrator.getEstimatedSx());
16491 assertNull(calibrator.getEstimatedSy());
16492 assertNull(calibrator.getEstimatedSz());
16493 assertNull(calibrator.getEstimatedMxy());
16494 assertNull(calibrator.getEstimatedMxz());
16495 assertNull(calibrator.getEstimatedMyx());
16496 assertNull(calibrator.getEstimatedMyz());
16497 assertNull(calibrator.getEstimatedMzx());
16498 assertNull(calibrator.getEstimatedMzy());
16499 assertNull(calibrator.getEstimatedCovariance());
16500 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16501 assertNotNull(calibrator.getGroundTruthGravityNorm());
16502 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16503 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16504 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16505 ABSOLUTE_ERROR));
16506 final var g = new Acceleration(0.0, AccelerationUnit.G);
16507 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16508 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16509 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16510
16511
16512 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16513 -gravityNorm, true, ba));
16514 final var m1 = new Matrix(1, 1);
16515 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16516 gravityNorm, true, m1));
16517 final var m2 = new Matrix(1, 3);
16518 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16519 gravityNorm, true, m2));
16520 }
16521
16522 @Test
16523 void testConstructor146() throws WrongSizeException {
16524 final var ba = generateBa();
16525 final var bias = ba.getBuffer();
16526 final var biasX = ba.getElementAtIndex(0);
16527 final var biasY = ba.getElementAtIndex(1);
16528 final var biasZ = ba.getElementAtIndex(2);
16529
16530 final var randomizer = new UniformRandomizer();
16531 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16532 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16533 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16534 final var nedPosition = new NEDPosition(latitude, longitude, height);
16535 final var nedVelocity = new NEDVelocity();
16536 final var ecefPosition = new ECEFPosition();
16537 final var ecefVelocity = new ECEFVelocity();
16538 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16539 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16540 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16541 final var gravityNorm = gravity.getNorm();
16542
16543 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, ba,
16544 this);
16545
16546
16547 assertEquals(biasX, calibrator.getBiasX(), 0.0);
16548 assertEquals(biasY, calibrator.getBiasY(), 0.0);
16549 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16550 final var bx1 = calibrator.getBiasXAsAcceleration();
16551 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16552 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16553 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16554 calibrator.getBiasXAsAcceleration(bx2);
16555 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16556 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16557 final var by1 = calibrator.getBiasYAsAcceleration();
16558 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16559 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16560 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16561 calibrator.getBiasYAsAcceleration(by2);
16562 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16563 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16564 final var bz1 = calibrator.getBiasZAsAcceleration();
16565 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16566 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16567 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16568 calibrator.getBiasZAsAcceleration(bz2);
16569 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16570 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16571 final var biasTriad1 = calibrator.getBiasAsTriad();
16572 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16573 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16574 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16575 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16576 final var biasTriad2 = new AccelerationTriad();
16577 calibrator.getBiasAsTriad(biasTriad2);
16578 assertEquals(biasTriad1, biasTriad2);
16579 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
16580 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
16581 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
16582 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
16583 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
16584 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
16585 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
16586 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
16587 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
16588 final var bias1 = calibrator.getBias();
16589 assertArrayEquals(bias1, bias, 0.0);
16590 final var bias2 = new double[3];
16591 calibrator.getBias(bias2);
16592 assertArrayEquals(bias1, bias2, 0.0);
16593 final var b1 = calibrator.getBiasAsMatrix();
16594 assertEquals(b1, ba);
16595 final var b2 = new Matrix(3, 1);
16596 calibrator.getBiasAsMatrix(b2);
16597 assertEquals(b1, b2);
16598 final var ma1 = calibrator.getInitialMa();
16599 assertEquals(new Matrix(3, 3), ma1);
16600 final var ma2 = new Matrix(3, 3);
16601 calibrator.getInitialMa(ma2);
16602 assertEquals(ma1, ma2);
16603 assertNull(calibrator.getMeasurements());
16604 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16605 calibrator.getMeasurementType());
16606 assertFalse(calibrator.isOrderedMeasurementsRequired());
16607 assertFalse(calibrator.isQualityScoresRequired());
16608 assertTrue(calibrator.isCommonAxisUsed());
16609 assertSame(this, calibrator.getListener());
16610 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
16611 assertFalse(calibrator.isReady());
16612 assertFalse(calibrator.isRunning());
16613 assertNull(calibrator.getEstimatedMa());
16614 assertNull(calibrator.getEstimatedSx());
16615 assertNull(calibrator.getEstimatedSy());
16616 assertNull(calibrator.getEstimatedSz());
16617 assertNull(calibrator.getEstimatedMxy());
16618 assertNull(calibrator.getEstimatedMxz());
16619 assertNull(calibrator.getEstimatedMyx());
16620 assertNull(calibrator.getEstimatedMyz());
16621 assertNull(calibrator.getEstimatedMzx());
16622 assertNull(calibrator.getEstimatedMzy());
16623 assertNull(calibrator.getEstimatedCovariance());
16624 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16625 assertNotNull(calibrator.getGroundTruthGravityNorm());
16626 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16627 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16628 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16629 ABSOLUTE_ERROR));
16630 final var g = new Acceleration(0.0, AccelerationUnit.G);
16631 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16632 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16633 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16634
16635
16636 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16637 -gravityNorm, true, ba, this));
16638 final var m1 = new Matrix(1, 1);
16639 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16640 gravityNorm, true, m1, this));
16641 final var m2 = new Matrix(1, 3);
16642 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16643 gravityNorm, true, m2, this));
16644 }
16645
16646 @Test
16647 void testConstructor147() throws WrongSizeException {
16648 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
16649
16650 final var ba = generateBa();
16651 final var bias = ba.getBuffer();
16652 final var biasX = ba.getElementAtIndex(0);
16653 final var biasY = ba.getElementAtIndex(1);
16654 final var biasZ = ba.getElementAtIndex(2);
16655
16656 final var randomizer = new UniformRandomizer();
16657 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16658 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16659 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16660 final var nedPosition = new NEDPosition(latitude, longitude, height);
16661 final var nedVelocity = new NEDVelocity();
16662 final var ecefPosition = new ECEFPosition();
16663 final var ecefVelocity = new ECEFVelocity();
16664 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16665 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16666 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16667 final var gravityNorm = gravity.getNorm();
16668
16669 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
16670 true, ba);
16671
16672
16673 assertEquals(biasX, calibrator.getBiasX(), 0.0);
16674 assertEquals(biasY, calibrator.getBiasY(), 0.0);
16675 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16676 final var bx1 = calibrator.getBiasXAsAcceleration();
16677 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16678 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16679 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16680 calibrator.getBiasXAsAcceleration(bx2);
16681 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16682 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16683 final var by1 = calibrator.getBiasYAsAcceleration();
16684 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16685 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16686 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16687 calibrator.getBiasYAsAcceleration(by2);
16688 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16689 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16690 final var bz1 = calibrator.getBiasZAsAcceleration();
16691 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16692 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16693 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16694 calibrator.getBiasZAsAcceleration(bz2);
16695 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16696 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16697 final var biasTriad1 = calibrator.getBiasAsTriad();
16698 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16699 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16700 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16701 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16702 final var biasTriad2 = new AccelerationTriad();
16703 calibrator.getBiasAsTriad(biasTriad2);
16704 assertEquals(biasTriad1, biasTriad2);
16705 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
16706 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
16707 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
16708 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
16709 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
16710 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
16711 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
16712 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
16713 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
16714 final var bias1 = calibrator.getBias();
16715 assertArrayEquals(bias1, bias, 0.0);
16716 final var bias2 = new double[3];
16717 calibrator.getBias(bias2);
16718 assertArrayEquals(bias1, bias2, 0.0);
16719 final var b1 = calibrator.getBiasAsMatrix();
16720 assertEquals(b1, ba);
16721 final var b2 = new Matrix(3, 1);
16722 calibrator.getBiasAsMatrix(b2);
16723 assertEquals(b1, b2);
16724 final var ma1 = calibrator.getInitialMa();
16725 assertEquals(new Matrix(3, 3), ma1);
16726 final var ma2 = new Matrix(3, 3);
16727 calibrator.getInitialMa(ma2);
16728 assertEquals(ma1, ma2);
16729 assertSame(measurements, calibrator.getMeasurements());
16730 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16731 calibrator.getMeasurementType());
16732 assertFalse(calibrator.isOrderedMeasurementsRequired());
16733 assertFalse(calibrator.isQualityScoresRequired());
16734 assertTrue(calibrator.isCommonAxisUsed());
16735 assertNull(calibrator.getListener());
16736 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
16737 assertFalse(calibrator.isReady());
16738 assertFalse(calibrator.isRunning());
16739 assertNull(calibrator.getEstimatedMa());
16740 assertNull(calibrator.getEstimatedSx());
16741 assertNull(calibrator.getEstimatedSy());
16742 assertNull(calibrator.getEstimatedSz());
16743 assertNull(calibrator.getEstimatedMxy());
16744 assertNull(calibrator.getEstimatedMxz());
16745 assertNull(calibrator.getEstimatedMyx());
16746 assertNull(calibrator.getEstimatedMyz());
16747 assertNull(calibrator.getEstimatedMzx());
16748 assertNull(calibrator.getEstimatedMzy());
16749 assertNull(calibrator.getEstimatedCovariance());
16750 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16751 assertNotNull(calibrator.getGroundTruthGravityNorm());
16752 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16753 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16754 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16755 ABSOLUTE_ERROR));
16756 final var g = new Acceleration(0.0, AccelerationUnit.G);
16757 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16758 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16759 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16760
16761
16762 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16763 -gravityNorm, measurements, true, ba));
16764 final var m1 = new Matrix(1, 1);
16765 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16766 gravityNorm, measurements, true, m1));
16767 final var m2 = new Matrix(1, 3);
16768 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16769 gravityNorm, measurements, true, m2));
16770 }
16771
16772 @Test
16773 void testConstructor148() throws WrongSizeException {
16774 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
16775
16776 final var ba = generateBa();
16777 final var bias = ba.getBuffer();
16778 final var biasX = ba.getElementAtIndex(0);
16779 final var biasY = ba.getElementAtIndex(1);
16780 final var biasZ = ba.getElementAtIndex(2);
16781
16782 final var randomizer = new UniformRandomizer();
16783 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16784 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16785 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16786 final var nedPosition = new NEDPosition(latitude, longitude, height);
16787 final var nedVelocity = new NEDVelocity();
16788 final var ecefPosition = new ECEFPosition();
16789 final var ecefVelocity = new ECEFVelocity();
16790 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16791 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16792 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16793 final var gravityNorm = gravity.getNorm();
16794
16795 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
16796 true, ba, this);
16797
16798
16799 assertEquals(biasX, calibrator.getBiasX(), 0.0);
16800 assertEquals(biasY, calibrator.getBiasY(), 0.0);
16801 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16802 final var bx1 = calibrator.getBiasXAsAcceleration();
16803 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16804 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16805 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16806 calibrator.getBiasXAsAcceleration(bx2);
16807 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16808 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16809 final var by1 = calibrator.getBiasYAsAcceleration();
16810 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16811 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16812 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16813 calibrator.getBiasYAsAcceleration(by2);
16814 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16815 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16816 final var bz1 = calibrator.getBiasZAsAcceleration();
16817 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16818 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16819 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16820 calibrator.getBiasZAsAcceleration(bz2);
16821 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16822 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16823 final var biasTriad1 = calibrator.getBiasAsTriad();
16824 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16825 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16826 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16827 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16828 final var biasTriad2 = new AccelerationTriad();
16829 calibrator.getBiasAsTriad(biasTriad2);
16830 assertEquals(biasTriad1, biasTriad2);
16831 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
16832 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
16833 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
16834 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
16835 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
16836 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
16837 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
16838 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
16839 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
16840 final var bias1 = calibrator.getBias();
16841 assertArrayEquals(bias1, bias, 0.0);
16842 final var bias2 = new double[3];
16843 calibrator.getBias(bias2);
16844 assertArrayEquals(bias1, bias2, 0.0);
16845 final var b1 = calibrator.getBiasAsMatrix();
16846 assertEquals(b1, ba);
16847 final var b2 = new Matrix(3, 1);
16848 calibrator.getBiasAsMatrix(b2);
16849 assertEquals(b1, b2);
16850 final var ma1 = calibrator.getInitialMa();
16851 assertEquals(new Matrix(3, 3), ma1);
16852 final var ma2 = new Matrix(3, 3);
16853 calibrator.getInitialMa(ma2);
16854 assertEquals(ma1, ma2);
16855 assertSame(measurements, calibrator.getMeasurements());
16856 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16857 calibrator.getMeasurementType());
16858 assertFalse(calibrator.isOrderedMeasurementsRequired());
16859 assertFalse(calibrator.isQualityScoresRequired());
16860 assertTrue(calibrator.isCommonAxisUsed());
16861 assertSame(this, calibrator.getListener());
16862 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
16863 assertFalse(calibrator.isReady());
16864 assertFalse(calibrator.isRunning());
16865 assertNull(calibrator.getEstimatedMa());
16866 assertNull(calibrator.getEstimatedSx());
16867 assertNull(calibrator.getEstimatedSy());
16868 assertNull(calibrator.getEstimatedSz());
16869 assertNull(calibrator.getEstimatedMxy());
16870 assertNull(calibrator.getEstimatedMxz());
16871 assertNull(calibrator.getEstimatedMyx());
16872 assertNull(calibrator.getEstimatedMyz());
16873 assertNull(calibrator.getEstimatedMzx());
16874 assertNull(calibrator.getEstimatedMzy());
16875 assertNull(calibrator.getEstimatedCovariance());
16876 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16877 assertNotNull(calibrator.getGroundTruthGravityNorm());
16878 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16879 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16880 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16881 ABSOLUTE_ERROR));
16882 final var g = new Acceleration(0.0, AccelerationUnit.G);
16883 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16884 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16885 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16886
16887
16888 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16889 -gravityNorm, measurements, true, ba, this));
16890 final var m1 = new Matrix(1, 1);
16891 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16892 gravityNorm, measurements, true, m1, this));
16893 final var m2 = new Matrix(1, 3);
16894 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16895 gravityNorm, measurements, true, m2, this));
16896 }
16897
16898 @Test
16899 void testConstructor149() throws WrongSizeException {
16900 final var ba = generateBa();
16901 final var bias = ba.getBuffer();
16902 final var biasX = ba.getElementAtIndex(0);
16903 final var biasY = ba.getElementAtIndex(1);
16904 final var biasZ = ba.getElementAtIndex(2);
16905
16906 final var ma = generateMaCommonAxis();
16907 final var sx = ma.getElementAt(0, 0);
16908 final var sy = ma.getElementAt(1, 1);
16909 final var sz = ma.getElementAt(2, 2);
16910 final var mxy = ma.getElementAt(0, 1);
16911 final var mxz = ma.getElementAt(0, 2);
16912 final var myx = ma.getElementAt(1, 0);
16913 final var myz = ma.getElementAt(1, 2);
16914 final var mzx = ma.getElementAt(2, 0);
16915 final var mzy = ma.getElementAt(2, 1);
16916
16917 final var randomizer = new UniformRandomizer();
16918 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16919 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16920 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16921 final var nedPosition = new NEDPosition(latitude, longitude, height);
16922 final var nedVelocity = new NEDVelocity();
16923 final var ecefPosition = new ECEFPosition();
16924 final var ecefVelocity = new ECEFVelocity();
16925 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16926 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16927 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16928 final var gravityNorm = gravity.getNorm();
16929
16930 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba, ma);
16931
16932
16933 assertEquals(biasX, calibrator.getBiasX(), 0.0);
16934 assertEquals(biasY, calibrator.getBiasY(), 0.0);
16935 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16936 final var bx1 = calibrator.getBiasXAsAcceleration();
16937 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16938 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16939 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16940 calibrator.getBiasXAsAcceleration(bx2);
16941 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16942 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16943 final var by1 = calibrator.getBiasYAsAcceleration();
16944 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16945 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16946 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16947 calibrator.getBiasYAsAcceleration(by2);
16948 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16949 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16950 final var bz1 = calibrator.getBiasZAsAcceleration();
16951 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16952 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16953 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16954 calibrator.getBiasZAsAcceleration(bz2);
16955 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16956 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16957 final var biasTriad1 = calibrator.getBiasAsTriad();
16958 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16959 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16960 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16961 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16962 final var biasTriad2 = new AccelerationTriad();
16963 calibrator.getBiasAsTriad(biasTriad2);
16964 assertEquals(biasTriad1, biasTriad2);
16965 assertEquals(sx, calibrator.getInitialSx(), 0.0);
16966 assertEquals(sy, calibrator.getInitialSy(), 0.0);
16967 assertEquals(sz, calibrator.getInitialSz(), 0.0);
16968 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
16969 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
16970 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
16971 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
16972 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
16973 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
16974 final var bias1 = calibrator.getBias();
16975 assertArrayEquals(bias1, bias, 0.0);
16976 final var bias2 = new double[3];
16977 calibrator.getBias(bias2);
16978 assertArrayEquals(bias1, bias2, 0.0);
16979 final var b1 = calibrator.getBiasAsMatrix();
16980 assertEquals(b1, ba);
16981 final var b2 = new Matrix(3, 1);
16982 calibrator.getBiasAsMatrix(b2);
16983 assertEquals(b1, b2);
16984 final var ma1 = new Matrix(3, 3);
16985 ma1.setSubmatrix(0, 0, 2, 2,
16986 new double[]{sx, myx, mzx,
16987 mxy, sy, mzy,
16988 mxz, myz, sz});
16989 assertEquals(ma1, calibrator.getInitialMa());
16990 final var ma2 = new Matrix(3, 3);
16991 calibrator.getInitialMa(ma2);
16992 assertEquals(ma1, ma2);
16993 assertNull(calibrator.getMeasurements());
16994 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16995 calibrator.getMeasurementType());
16996 assertFalse(calibrator.isOrderedMeasurementsRequired());
16997 assertFalse(calibrator.isQualityScoresRequired());
16998 assertFalse(calibrator.isCommonAxisUsed());
16999 assertNull(calibrator.getListener());
17000 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
17001 assertFalse(calibrator.isReady());
17002 assertFalse(calibrator.isRunning());
17003 assertNull(calibrator.getEstimatedMa());
17004 assertNull(calibrator.getEstimatedSx());
17005 assertNull(calibrator.getEstimatedSy());
17006 assertNull(calibrator.getEstimatedSz());
17007 assertNull(calibrator.getEstimatedMxy());
17008 assertNull(calibrator.getEstimatedMxz());
17009 assertNull(calibrator.getEstimatedMyx());
17010 assertNull(calibrator.getEstimatedMyz());
17011 assertNull(calibrator.getEstimatedMzx());
17012 assertNull(calibrator.getEstimatedMzy());
17013 assertNull(calibrator.getEstimatedCovariance());
17014 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
17015 assertNotNull(calibrator.getGroundTruthGravityNorm());
17016 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17017 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17018 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
17019 ABSOLUTE_ERROR));
17020 final var g = new Acceleration(0.0, AccelerationUnit.G);
17021 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17022 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17023 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
17024
17025
17026 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17027 -gravityNorm, ba, ma));
17028 final var m1 = new Matrix(1, 1);
17029 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17030 gravityNorm, m1, ma));
17031 final var m2 = new Matrix(1, 3);
17032 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17033 gravityNorm, m2, ma));
17034 final var m3 = new Matrix(1, 3);
17035 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17036 gravityNorm, ba, m3));
17037 final var m4 = new Matrix(3, 1);
17038 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17039 gravityNorm, ba, m4));
17040 }
17041
17042 @Test
17043 void testConstructor150() throws WrongSizeException {
17044 final var ba = generateBa();
17045 final var bias = ba.getBuffer();
17046 final var biasX = ba.getElementAtIndex(0);
17047 final var biasY = ba.getElementAtIndex(1);
17048 final var biasZ = ba.getElementAtIndex(2);
17049
17050 final var ma = generateMaCommonAxis();
17051 final var sx = ma.getElementAt(0, 0);
17052 final var sy = ma.getElementAt(1, 1);
17053 final var sz = ma.getElementAt(2, 2);
17054 final var mxy = ma.getElementAt(0, 1);
17055 final var mxz = ma.getElementAt(0, 2);
17056 final var myx = ma.getElementAt(1, 0);
17057 final var myz = ma.getElementAt(1, 2);
17058 final var mzx = ma.getElementAt(2, 0);
17059 final var mzy = ma.getElementAt(2, 1);
17060
17061 final var randomizer = new UniformRandomizer();
17062 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17063 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17064 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17065 final var nedPosition = new NEDPosition(latitude, longitude, height);
17066 final var nedVelocity = new NEDVelocity();
17067 final var ecefPosition = new ECEFPosition();
17068 final var ecefVelocity = new ECEFVelocity();
17069 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
17070 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17071 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17072 final var gravityNorm = gravity.getNorm();
17073
17074 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba, ma, this);
17075
17076
17077 assertEquals(biasX, calibrator.getBiasX(), 0.0);
17078 assertEquals(biasY, calibrator.getBiasY(), 0.0);
17079 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
17080 final var bx1 = calibrator.getBiasXAsAcceleration();
17081 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
17082 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
17083 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17084 calibrator.getBiasXAsAcceleration(bx2);
17085 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
17086 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
17087 final var by1 = calibrator.getBiasYAsAcceleration();
17088 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
17089 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
17090 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17091 calibrator.getBiasYAsAcceleration(by2);
17092 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
17093 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
17094 final var bz1 = calibrator.getBiasZAsAcceleration();
17095 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
17096 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
17097 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17098 calibrator.getBiasZAsAcceleration(bz2);
17099 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
17100 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
17101 final var biasTriad1 = calibrator.getBiasAsTriad();
17102 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
17103 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
17104 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
17105 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
17106 final var biasTriad2 = new AccelerationTriad();
17107 calibrator.getBiasAsTriad(biasTriad2);
17108 assertEquals(biasTriad1, biasTriad2);
17109 assertEquals(sx, calibrator.getInitialSx(), 0.0);
17110 assertEquals(sy, calibrator.getInitialSy(), 0.0);
17111 assertEquals(sz, calibrator.getInitialSz(), 0.0);
17112 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
17113 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
17114 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
17115 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
17116 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
17117 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
17118 final var bias1 = calibrator.getBias();
17119 assertArrayEquals(bias1, bias, 0.0);
17120 final var bias2 = new double[3];
17121 calibrator.getBias(bias2);
17122 assertArrayEquals(bias1, bias2, 0.0);
17123 final var b1 = calibrator.getBiasAsMatrix();
17124 assertEquals(b1, ba);
17125 final var b2 = new Matrix(3, 1);
17126 calibrator.getBiasAsMatrix(b2);
17127 assertEquals(b1, b2);
17128 final var ma1 = new Matrix(3, 3);
17129 ma1.setSubmatrix(0, 0, 2, 2,
17130 new double[]{sx, myx, mzx,
17131 mxy, sy, mzy,
17132 mxz, myz, sz});
17133 assertEquals(ma1, calibrator.getInitialMa());
17134 final var ma2 = new Matrix(3, 3);
17135 calibrator.getInitialMa(ma2);
17136 assertEquals(ma1, ma2);
17137 assertNull(calibrator.getMeasurements());
17138 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
17139 calibrator.getMeasurementType());
17140 assertFalse(calibrator.isOrderedMeasurementsRequired());
17141 assertFalse(calibrator.isQualityScoresRequired());
17142 assertFalse(calibrator.isCommonAxisUsed());
17143 assertSame(this, calibrator.getListener());
17144 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
17145 assertFalse(calibrator.isReady());
17146 assertFalse(calibrator.isRunning());
17147 assertNull(calibrator.getEstimatedMa());
17148 assertNull(calibrator.getEstimatedSx());
17149 assertNull(calibrator.getEstimatedSy());
17150 assertNull(calibrator.getEstimatedSz());
17151 assertNull(calibrator.getEstimatedMxy());
17152 assertNull(calibrator.getEstimatedMxz());
17153 assertNull(calibrator.getEstimatedMyx());
17154 assertNull(calibrator.getEstimatedMyz());
17155 assertNull(calibrator.getEstimatedMzx());
17156 assertNull(calibrator.getEstimatedMzy());
17157 assertNull(calibrator.getEstimatedCovariance());
17158 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
17159 assertNotNull(calibrator.getGroundTruthGravityNorm());
17160 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17161 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17162 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
17163 ABSOLUTE_ERROR));
17164 final var g = new Acceleration(0.0, AccelerationUnit.G);
17165 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17166 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17167 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
17168
17169
17170 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17171 -gravityNorm, ba, ma, this));
17172 final var m1 = new Matrix(1, 1);
17173 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17174 gravityNorm, m1, ma, this));
17175 final var m2 = new Matrix(1, 3);
17176 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17177 gravityNorm, m2, ma, this));
17178 final var m3 = new Matrix(1, 3);
17179 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17180 gravityNorm, ba, m3, this));
17181 final var m4 = new Matrix(3, 1);
17182 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17183 gravityNorm, ba, m4, this));
17184 }
17185
17186 @Test
17187 void testConstructor151() throws WrongSizeException {
17188 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
17189
17190 final var ba = generateBa();
17191 final var bias = ba.getBuffer();
17192 final var biasX = ba.getElementAtIndex(0);
17193 final var biasY = ba.getElementAtIndex(1);
17194 final var biasZ = ba.getElementAtIndex(2);
17195
17196 final var ma = generateMaCommonAxis();
17197 final var sx = ma.getElementAt(0, 0);
17198 final var sy = ma.getElementAt(1, 1);
17199 final var sz = ma.getElementAt(2, 2);
17200 final var mxy = ma.getElementAt(0, 1);
17201 final var mxz = ma.getElementAt(0, 2);
17202 final var myx = ma.getElementAt(1, 0);
17203 final var myz = ma.getElementAt(1, 2);
17204 final var mzx = ma.getElementAt(2, 0);
17205 final var mzy = ma.getElementAt(2, 1);
17206
17207 final var randomizer = new UniformRandomizer();
17208 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17209 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17210 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17211 final var nedPosition = new NEDPosition(latitude, longitude, height);
17212 final var nedVelocity = new NEDVelocity();
17213 final var ecefPosition = new ECEFPosition();
17214 final var ecefVelocity = new ECEFVelocity();
17215 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
17216 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17217 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17218 final var gravityNorm = gravity.getNorm();
17219
17220 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba, ma);
17221
17222
17223 assertEquals(biasX, calibrator.getBiasX(), 0.0);
17224 assertEquals(biasY, calibrator.getBiasY(), 0.0);
17225 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
17226 final var bx1 = calibrator.getBiasXAsAcceleration();
17227 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
17228 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
17229 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17230 calibrator.getBiasXAsAcceleration(bx2);
17231 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
17232 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
17233 final var by1 = calibrator.getBiasYAsAcceleration();
17234 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
17235 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
17236 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17237 calibrator.getBiasYAsAcceleration(by2);
17238 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
17239 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
17240 final var bz1 = calibrator.getBiasZAsAcceleration();
17241 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
17242 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
17243 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17244 calibrator.getBiasZAsAcceleration(bz2);
17245 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
17246 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
17247 final var biasTriad1 = calibrator.getBiasAsTriad();
17248 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
17249 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
17250 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
17251 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
17252 final var biasTriad2 = new AccelerationTriad();
17253 calibrator.getBiasAsTriad(biasTriad2);
17254 assertEquals(biasTriad1, biasTriad2);
17255 assertEquals(sx, calibrator.getInitialSx(), 0.0);
17256 assertEquals(sy, calibrator.getInitialSy(), 0.0);
17257 assertEquals(sz, calibrator.getInitialSz(), 0.0);
17258 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
17259 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
17260 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
17261 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
17262 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
17263 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
17264 final var bias1 = calibrator.getBias();
17265 assertArrayEquals(bias1, bias, 0.0);
17266 final var bias2 = new double[3];
17267 calibrator.getBias(bias2);
17268 assertArrayEquals(bias1, bias2, 0.0);
17269 final var b1 = calibrator.getBiasAsMatrix();
17270 assertEquals(b1, ba);
17271 final var b2 = new Matrix(3, 1);
17272 calibrator.getBiasAsMatrix(b2);
17273 assertEquals(b1, b2);
17274 final var ma1 = new Matrix(3, 3);
17275 ma1.setSubmatrix(0, 0, 2, 2,
17276 new double[]{sx, myx, mzx,
17277 mxy, sy, mzy,
17278 mxz, myz, sz});
17279 assertEquals(ma1, calibrator.getInitialMa());
17280 final var ma2 = new Matrix(3, 3);
17281 calibrator.getInitialMa(ma2);
17282 assertEquals(ma1, ma2);
17283 assertSame(measurements, calibrator.getMeasurements());
17284 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
17285 calibrator.getMeasurementType());
17286 assertFalse(calibrator.isOrderedMeasurementsRequired());
17287 assertFalse(calibrator.isQualityScoresRequired());
17288 assertFalse(calibrator.isCommonAxisUsed());
17289 assertNull(calibrator.getListener());
17290 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
17291 assertFalse(calibrator.isReady());
17292 assertFalse(calibrator.isRunning());
17293 assertNull(calibrator.getEstimatedMa());
17294 assertNull(calibrator.getEstimatedSx());
17295 assertNull(calibrator.getEstimatedSy());
17296 assertNull(calibrator.getEstimatedSz());
17297 assertNull(calibrator.getEstimatedMxy());
17298 assertNull(calibrator.getEstimatedMxz());
17299 assertNull(calibrator.getEstimatedMyx());
17300 assertNull(calibrator.getEstimatedMyz());
17301 assertNull(calibrator.getEstimatedMzx());
17302 assertNull(calibrator.getEstimatedMzy());
17303 assertNull(calibrator.getEstimatedCovariance());
17304 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
17305 assertNotNull(calibrator.getGroundTruthGravityNorm());
17306 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17307 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17308 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
17309 ABSOLUTE_ERROR));
17310 final var g = new Acceleration(0.0, AccelerationUnit.G);
17311 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17312 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17313 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
17314
17315
17316 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17317 -gravityNorm, measurements, ba, ma));
17318 final var m1 = new Matrix(1, 1);
17319 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17320 gravityNorm, measurements, m1, ma));
17321 final var m2 = new Matrix(1, 3);
17322 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17323 gravityNorm, measurements, m2, ma));
17324 final var m3 = new Matrix(1, 3);
17325 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17326 gravityNorm, measurements, ba, m3));
17327 final var m4 = new Matrix(3, 1);
17328 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17329 gravityNorm, measurements, ba, m4));
17330 }
17331
17332 @Test
17333 void testConstructor152() throws WrongSizeException {
17334 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
17335
17336 final var ba = generateBa();
17337 final var bias = ba.getBuffer();
17338 final var biasX = ba.getElementAtIndex(0);
17339 final var biasY = ba.getElementAtIndex(1);
17340 final var biasZ = ba.getElementAtIndex(2);
17341
17342 final var ma = generateMaCommonAxis();
17343 final var sx = ma.getElementAt(0, 0);
17344 final var sy = ma.getElementAt(1, 1);
17345 final var sz = ma.getElementAt(2, 2);
17346 final var mxy = ma.getElementAt(0, 1);
17347 final var mxz = ma.getElementAt(0, 2);
17348 final var myx = ma.getElementAt(1, 0);
17349 final var myz = ma.getElementAt(1, 2);
17350 final var mzx = ma.getElementAt(2, 0);
17351 final var mzy = ma.getElementAt(2, 1);
17352
17353 final var randomizer = new UniformRandomizer();
17354 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17355 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17356 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17357 final var nedPosition = new NEDPosition(latitude, longitude, height);
17358 final var nedVelocity = new NEDVelocity();
17359 final var ecefPosition = new ECEFPosition();
17360 final var ecefVelocity = new ECEFVelocity();
17361 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
17362 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17363 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17364 final var gravityNorm = gravity.getNorm();
17365
17366 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba, ma,
17367 this);
17368
17369
17370 assertEquals(biasX, calibrator.getBiasX(), 0.0);
17371 assertEquals(biasY, calibrator.getBiasY(), 0.0);
17372 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
17373 final var bx1 = calibrator.getBiasXAsAcceleration();
17374 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
17375 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
17376 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17377 calibrator.getBiasXAsAcceleration(bx2);
17378 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
17379 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
17380 final var by1 = calibrator.getBiasYAsAcceleration();
17381 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
17382 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
17383 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17384 calibrator.getBiasYAsAcceleration(by2);
17385 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
17386 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
17387 final var bz1 = calibrator.getBiasZAsAcceleration();
17388 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
17389 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
17390 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17391 calibrator.getBiasZAsAcceleration(bz2);
17392 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
17393 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
17394 final var biasTriad1 = calibrator.getBiasAsTriad();
17395 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
17396 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
17397 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
17398 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
17399 final var biasTriad2 = new AccelerationTriad();
17400 calibrator.getBiasAsTriad(biasTriad2);
17401 assertEquals(biasTriad1, biasTriad2);
17402 assertEquals(sx, calibrator.getInitialSx(), 0.0);
17403 assertEquals(sy, calibrator.getInitialSy(), 0.0);
17404 assertEquals(sz, calibrator.getInitialSz(), 0.0);
17405 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
17406 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
17407 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
17408 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
17409 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
17410 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
17411 final var bias1 = calibrator.getBias();
17412 assertArrayEquals(bias1, bias, 0.0);
17413 final var bias2 = new double[3];
17414 calibrator.getBias(bias2);
17415 assertArrayEquals(bias1, bias2, 0.0);
17416 final var b1 = calibrator.getBiasAsMatrix();
17417 assertEquals(b1, ba);
17418 final var b2 = new Matrix(3, 1);
17419 calibrator.getBiasAsMatrix(b2);
17420 assertEquals(b1, b2);
17421 final var ma1 = new Matrix(3, 3);
17422 ma1.setSubmatrix(0, 0, 2, 2,
17423 new double[]{sx, myx, mzx,
17424 mxy, sy, mzy,
17425 mxz, myz, sz});
17426 assertEquals(ma1, calibrator.getInitialMa());
17427 final var ma2 = new Matrix(3, 3);
17428 calibrator.getInitialMa(ma2);
17429 assertEquals(ma1, ma2);
17430 assertSame(measurements, calibrator.getMeasurements());
17431 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
17432 calibrator.getMeasurementType());
17433 assertFalse(calibrator.isOrderedMeasurementsRequired());
17434 assertFalse(calibrator.isQualityScoresRequired());
17435 assertFalse(calibrator.isCommonAxisUsed());
17436 assertSame(this, calibrator.getListener());
17437 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
17438 assertFalse(calibrator.isReady());
17439 assertFalse(calibrator.isRunning());
17440 assertNull(calibrator.getEstimatedMa());
17441 assertNull(calibrator.getEstimatedSx());
17442 assertNull(calibrator.getEstimatedSy());
17443 assertNull(calibrator.getEstimatedSz());
17444 assertNull(calibrator.getEstimatedMxy());
17445 assertNull(calibrator.getEstimatedMxz());
17446 assertNull(calibrator.getEstimatedMyx());
17447 assertNull(calibrator.getEstimatedMyz());
17448 assertNull(calibrator.getEstimatedMzx());
17449 assertNull(calibrator.getEstimatedMzy());
17450 assertNull(calibrator.getEstimatedCovariance());
17451 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
17452 assertNotNull(calibrator.getGroundTruthGravityNorm());
17453 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17454 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17455 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
17456 ABSOLUTE_ERROR));
17457 final var g = new Acceleration(0.0, AccelerationUnit.G);
17458 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17459 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17460 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
17461
17462
17463 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17464 -gravityNorm, measurements, ba, ma, this));
17465 final var m1 = new Matrix(1, 1);
17466 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17467 gravityNorm, measurements, m1, ma, this));
17468 final var m2 = new Matrix(1, 3);
17469 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17470 gravityNorm, measurements, m2, ma, this));
17471 final var m3 = new Matrix(1, 3);
17472 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17473 gravityNorm, measurements, ba, m3, this));
17474 final var m4 = new Matrix(3, 1);
17475 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17476 gravityNorm, measurements, ba, m4, this));
17477 }
17478
17479 @Test
17480 void testConstructor153() throws WrongSizeException {
17481 final var ba = generateBa();
17482 final var bias = ba.getBuffer();
17483 final var biasX = ba.getElementAtIndex(0);
17484 final var biasY = ba.getElementAtIndex(1);
17485 final var biasZ = ba.getElementAtIndex(2);
17486
17487 final var ma = generateMaCommonAxis();
17488 final var sx = ma.getElementAt(0, 0);
17489 final var sy = ma.getElementAt(1, 1);
17490 final var sz = ma.getElementAt(2, 2);
17491 final var mxy = ma.getElementAt(0, 1);
17492 final var mxz = ma.getElementAt(0, 2);
17493 final var myx = ma.getElementAt(1, 0);
17494 final var myz = ma.getElementAt(1, 2);
17495 final var mzx = ma.getElementAt(2, 0);
17496 final var mzy = ma.getElementAt(2, 1);
17497
17498 final var randomizer = new UniformRandomizer();
17499 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17500 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17501 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17502 final var nedPosition = new NEDPosition(latitude, longitude, height);
17503 final var nedVelocity = new NEDVelocity();
17504 final var ecefPosition = new ECEFPosition();
17505 final var ecefVelocity = new ECEFVelocity();
17506 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
17507 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17508 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17509 final var gravityNorm = gravity.getNorm();
17510
17511 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, ba, ma);
17512
17513
17514 assertEquals(biasX, calibrator.getBiasX(), 0.0);
17515 assertEquals(biasY, calibrator.getBiasY(), 0.0);
17516 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
17517 final var bx1 = calibrator.getBiasXAsAcceleration();
17518 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
17519 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
17520 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17521 calibrator.getBiasXAsAcceleration(bx2);
17522 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
17523 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
17524 final var by1 = calibrator.getBiasYAsAcceleration();
17525 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
17526 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
17527 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17528 calibrator.getBiasYAsAcceleration(by2);
17529 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
17530 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
17531 final var bz1 = calibrator.getBiasZAsAcceleration();
17532 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
17533 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
17534 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17535 calibrator.getBiasZAsAcceleration(bz2);
17536 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
17537 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
17538 final var biasTriad1 = calibrator.getBiasAsTriad();
17539 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
17540 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
17541 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
17542 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
17543 final var biasTriad2 = new AccelerationTriad();
17544 calibrator.getBiasAsTriad(biasTriad2);
17545 assertEquals(biasTriad1, biasTriad2);
17546 assertEquals(sx, calibrator.getInitialSx(), 0.0);
17547 assertEquals(sy, calibrator.getInitialSy(), 0.0);
17548 assertEquals(sz, calibrator.getInitialSz(), 0.0);
17549 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
17550 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
17551 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
17552 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
17553 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
17554 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
17555 final var bias1 = calibrator.getBias();
17556 assertArrayEquals(bias1, bias, 0.0);
17557 final var bias2 = new double[3];
17558 calibrator.getBias(bias2);
17559 assertArrayEquals(bias1, bias2, 0.0);
17560 final var b1 = calibrator.getBiasAsMatrix();
17561 assertEquals(b1, ba);
17562 final var b2 = new Matrix(3, 1);
17563 calibrator.getBiasAsMatrix(b2);
17564 assertEquals(b1, b2);
17565 final var ma1 = new Matrix(3, 3);
17566 ma1.setSubmatrix(0, 0, 2, 2,
17567 new double[]{sx, myx, mzx,
17568 mxy, sy, mzy,
17569 mxz, myz, sz});
17570 assertEquals(ma1, calibrator.getInitialMa());
17571 final var ma2 = new Matrix(3, 3);
17572 calibrator.getInitialMa(ma2);
17573 assertEquals(ma1, ma2);
17574 assertNull(calibrator.getMeasurements());
17575 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
17576 calibrator.getMeasurementType());
17577 assertFalse(calibrator.isOrderedMeasurementsRequired());
17578 assertFalse(calibrator.isQualityScoresRequired());
17579 assertTrue(calibrator.isCommonAxisUsed());
17580 assertNull(calibrator.getListener());
17581 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
17582 assertFalse(calibrator.isReady());
17583 assertFalse(calibrator.isRunning());
17584 assertNull(calibrator.getEstimatedMa());
17585 assertNull(calibrator.getEstimatedSx());
17586 assertNull(calibrator.getEstimatedSy());
17587 assertNull(calibrator.getEstimatedSz());
17588 assertNull(calibrator.getEstimatedMxy());
17589 assertNull(calibrator.getEstimatedMxz());
17590 assertNull(calibrator.getEstimatedMyx());
17591 assertNull(calibrator.getEstimatedMyz());
17592 assertNull(calibrator.getEstimatedMzx());
17593 assertNull(calibrator.getEstimatedMzy());
17594 assertNull(calibrator.getEstimatedCovariance());
17595 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
17596 assertNotNull(calibrator.getGroundTruthGravityNorm());
17597 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17598 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17599 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
17600 ABSOLUTE_ERROR));
17601 final var g = new Acceleration(0.0, AccelerationUnit.G);
17602 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17603 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17604 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
17605
17606
17607 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17608 -gravityNorm, true, ba, ma));
17609 final var m1 = new Matrix(1, 1);
17610 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17611 gravityNorm, true, m1, ma));
17612 final var m2 = new Matrix(1, 3);
17613 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17614 gravityNorm, true, m2, ma));
17615 final var m3 = new Matrix(1, 3);
17616 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17617 gravityNorm, true, ba, m3));
17618 final var m4 = new Matrix(3, 1);
17619 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17620 gravityNorm, true, ba, m4));
17621 }
17622
17623 @Test
17624 void testConstructor154() throws WrongSizeException {
17625 final var ba = generateBa();
17626 final var bias = ba.getBuffer();
17627 final var biasX = ba.getElementAtIndex(0);
17628 final var biasY = ba.getElementAtIndex(1);
17629 final var biasZ = ba.getElementAtIndex(2);
17630
17631 final var ma = generateMaCommonAxis();
17632 final var sx = ma.getElementAt(0, 0);
17633 final var sy = ma.getElementAt(1, 1);
17634 final var sz = ma.getElementAt(2, 2);
17635 final var mxy = ma.getElementAt(0, 1);
17636 final var mxz = ma.getElementAt(0, 2);
17637 final var myx = ma.getElementAt(1, 0);
17638 final var myz = ma.getElementAt(1, 2);
17639 final var mzx = ma.getElementAt(2, 0);
17640 final var mzy = ma.getElementAt(2, 1);
17641
17642 final var randomizer = new UniformRandomizer();
17643 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17644 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17645 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17646 final var nedPosition = new NEDPosition(latitude, longitude, height);
17647 final var nedVelocity = new NEDVelocity();
17648 final var ecefPosition = new ECEFPosition();
17649 final var ecefVelocity = new ECEFVelocity();
17650 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
17651 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17652 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17653 final var gravityNorm = gravity.getNorm();
17654
17655 KnownBiasAndGravityNormAccelerometerCalibrator calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17656 gravityNorm, true, ba, ma, this);
17657
17658
17659 assertEquals(biasX, calibrator.getBiasX(), 0.0);
17660 assertEquals(biasY, calibrator.getBiasY(), 0.0);
17661 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
17662 final var bx1 = calibrator.getBiasXAsAcceleration();
17663 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
17664 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
17665 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17666 calibrator.getBiasXAsAcceleration(bx2);
17667 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
17668 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
17669 final var by1 = calibrator.getBiasYAsAcceleration();
17670 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
17671 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
17672 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17673 calibrator.getBiasYAsAcceleration(by2);
17674 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
17675 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
17676 final var bz1 = calibrator.getBiasZAsAcceleration();
17677 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
17678 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
17679 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17680 calibrator.getBiasZAsAcceleration(bz2);
17681 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
17682 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
17683 final var biasTriad1 = calibrator.getBiasAsTriad();
17684 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
17685 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
17686 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
17687 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
17688 final var biasTriad2 = new AccelerationTriad();
17689 calibrator.getBiasAsTriad(biasTriad2);
17690 assertEquals(biasTriad1, biasTriad2);
17691 assertEquals(sx, calibrator.getInitialSx(), 0.0);
17692 assertEquals(sy, calibrator.getInitialSy(), 0.0);
17693 assertEquals(sz, calibrator.getInitialSz(), 0.0);
17694 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
17695 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
17696 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
17697 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
17698 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
17699 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
17700 final var bias1 = calibrator.getBias();
17701 assertArrayEquals(bias1, bias, 0.0);
17702 final var bias2 = new double[3];
17703 calibrator.getBias(bias2);
17704 assertArrayEquals(bias1, bias2, 0.0);
17705 final var b1 = calibrator.getBiasAsMatrix();
17706 assertEquals(b1, ba);
17707 final var b2 = new Matrix(3, 1);
17708 calibrator.getBiasAsMatrix(b2);
17709 assertEquals(b1, b2);
17710 final var ma1 = new Matrix(3, 3);
17711 ma1.setSubmatrix(0, 0, 2, 2,
17712 new double[]{sx, myx, mzx,
17713 mxy, sy, mzy,
17714 mxz, myz, sz});
17715 assertEquals(ma1, calibrator.getInitialMa());
17716 final var ma2 = new Matrix(3, 3);
17717 calibrator.getInitialMa(ma2);
17718 assertEquals(ma1, ma2);
17719 assertNull(calibrator.getMeasurements());
17720 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
17721 calibrator.getMeasurementType());
17722 assertFalse(calibrator.isOrderedMeasurementsRequired());
17723 assertFalse(calibrator.isQualityScoresRequired());
17724 assertTrue(calibrator.isCommonAxisUsed());
17725 assertSame(this, calibrator.getListener());
17726 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
17727 assertFalse(calibrator.isReady());
17728 assertFalse(calibrator.isRunning());
17729 assertNull(calibrator.getEstimatedMa());
17730 assertNull(calibrator.getEstimatedSx());
17731 assertNull(calibrator.getEstimatedSy());
17732 assertNull(calibrator.getEstimatedSz());
17733 assertNull(calibrator.getEstimatedMxy());
17734 assertNull(calibrator.getEstimatedMxz());
17735 assertNull(calibrator.getEstimatedMyx());
17736 assertNull(calibrator.getEstimatedMyz());
17737 assertNull(calibrator.getEstimatedMzx());
17738 assertNull(calibrator.getEstimatedMzy());
17739 assertNull(calibrator.getEstimatedCovariance());
17740 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
17741 assertNotNull(calibrator.getGroundTruthGravityNorm());
17742 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17743 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17744 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
17745 ABSOLUTE_ERROR));
17746 final var g = new Acceleration(0.0, AccelerationUnit.G);
17747 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17748 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17749 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
17750
17751
17752 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17753 -gravityNorm, true, ba, ma, this));
17754 final var m1 = new Matrix(1, 1);
17755 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17756 gravityNorm, true, m1, ma, this));
17757 final var m2 = new Matrix(1, 3);
17758 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17759 gravityNorm, true, m2, ma, this));
17760 final var m3 = new Matrix(1, 3);
17761 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17762 gravityNorm, true, ba, m3, this));
17763 final var m4 = new Matrix(3, 1);
17764 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17765 gravityNorm, true, ba, m4, this));
17766 }
17767
17768 @Test
17769 void testConstructor155() throws WrongSizeException {
17770 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
17771
17772 final var ba = generateBa();
17773 final var bias = ba.getBuffer();
17774 final var biasX = ba.getElementAtIndex(0);
17775 final var biasY = ba.getElementAtIndex(1);
17776 final var biasZ = ba.getElementAtIndex(2);
17777
17778 final var ma = generateMaCommonAxis();
17779 final var sx = ma.getElementAt(0, 0);
17780 final var sy = ma.getElementAt(1, 1);
17781 final var sz = ma.getElementAt(2, 2);
17782 final var mxy = ma.getElementAt(0, 1);
17783 final var mxz = ma.getElementAt(0, 2);
17784 final var myx = ma.getElementAt(1, 0);
17785 final var myz = ma.getElementAt(1, 2);
17786 final var mzx = ma.getElementAt(2, 0);
17787 final var mzy = ma.getElementAt(2, 1);
17788
17789 final var randomizer = new UniformRandomizer();
17790 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17791 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17792 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17793 final var nedPosition = new NEDPosition(latitude, longitude, height);
17794 final var nedVelocity = new NEDVelocity();
17795 final var ecefPosition = new ECEFPosition();
17796 final var ecefVelocity = new ECEFVelocity();
17797 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
17798 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17799 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17800 final var gravityNorm = gravity.getNorm();
17801
17802 KnownBiasAndGravityNormAccelerometerCalibrator calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17803 gravityNorm, measurements, true, ba, ma);
17804
17805
17806 assertEquals(biasX, calibrator.getBiasX(), 0.0);
17807 assertEquals(biasY, calibrator.getBiasY(), 0.0);
17808 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
17809 final var bx1 = calibrator.getBiasXAsAcceleration();
17810 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
17811 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
17812 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17813 calibrator.getBiasXAsAcceleration(bx2);
17814 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
17815 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
17816 final var by1 = calibrator.getBiasYAsAcceleration();
17817 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
17818 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
17819 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17820 calibrator.getBiasYAsAcceleration(by2);
17821 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
17822 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
17823 final var bz1 = calibrator.getBiasZAsAcceleration();
17824 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
17825 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
17826 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17827 calibrator.getBiasZAsAcceleration(bz2);
17828 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
17829 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
17830 final var biasTriad1 = calibrator.getBiasAsTriad();
17831 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
17832 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
17833 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
17834 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
17835 final var biasTriad2 = new AccelerationTriad();
17836 calibrator.getBiasAsTriad(biasTriad2);
17837 assertEquals(biasTriad1, biasTriad2);
17838 assertEquals(sx, calibrator.getInitialSx(), 0.0);
17839 assertEquals(sy, calibrator.getInitialSy(), 0.0);
17840 assertEquals(sz, calibrator.getInitialSz(), 0.0);
17841 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
17842 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
17843 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
17844 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
17845 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
17846 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
17847 final var bias1 = calibrator.getBias();
17848 assertArrayEquals(bias1, bias, 0.0);
17849 final var bias2 = new double[3];
17850 calibrator.getBias(bias2);
17851 assertArrayEquals(bias1, bias2, 0.0);
17852 final var b1 = calibrator.getBiasAsMatrix();
17853 assertEquals(b1, ba);
17854 final var b2 = new Matrix(3, 1);
17855 calibrator.getBiasAsMatrix(b2);
17856 assertEquals(b1, b2);
17857 final var ma1 = new Matrix(3, 3);
17858 ma1.setSubmatrix(0, 0, 2, 2,
17859 new double[]{sx, myx, mzx,
17860 mxy, sy, mzy,
17861 mxz, myz, sz});
17862 assertEquals(ma1, calibrator.getInitialMa());
17863 final var ma2 = new Matrix(3, 3);
17864 calibrator.getInitialMa(ma2);
17865 assertEquals(ma1, ma2);
17866 assertSame(measurements, calibrator.getMeasurements());
17867 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
17868 calibrator.getMeasurementType());
17869 assertFalse(calibrator.isOrderedMeasurementsRequired());
17870 assertFalse(calibrator.isQualityScoresRequired());
17871 assertTrue(calibrator.isCommonAxisUsed());
17872 assertNull(calibrator.getListener());
17873 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
17874 assertFalse(calibrator.isReady());
17875 assertFalse(calibrator.isRunning());
17876 assertNull(calibrator.getEstimatedMa());
17877 assertNull(calibrator.getEstimatedSx());
17878 assertNull(calibrator.getEstimatedSy());
17879 assertNull(calibrator.getEstimatedSz());
17880 assertNull(calibrator.getEstimatedMxy());
17881 assertNull(calibrator.getEstimatedMxz());
17882 assertNull(calibrator.getEstimatedMyx());
17883 assertNull(calibrator.getEstimatedMyz());
17884 assertNull(calibrator.getEstimatedMzx());
17885 assertNull(calibrator.getEstimatedMzy());
17886 assertNull(calibrator.getEstimatedCovariance());
17887 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
17888 assertNotNull(calibrator.getGroundTruthGravityNorm());
17889 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17890 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17891 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
17892 ABSOLUTE_ERROR));
17893 final var g = new Acceleration(0.0, AccelerationUnit.G);
17894 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17895 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17896 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
17897
17898
17899 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17900 -gravityNorm, measurements, true, ba, ma));
17901 final var m1 = new Matrix(1, 1);
17902 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17903 gravityNorm, measurements, true, m1, ma));
17904 final var m2 = new Matrix(1, 3);
17905 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17906 gravityNorm, measurements, true, m2, ma));
17907 final var m3 = new Matrix(1, 3);
17908 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17909 gravityNorm, measurements, true, ba, m3));
17910 final var m4 = new Matrix(3, 1);
17911 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17912 gravityNorm, measurements, true, ba, m4));
17913 }
17914
17915 @Test
17916 void testConstructor156() throws WrongSizeException {
17917 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
17918
17919 final var ba = generateBa();
17920 final var bias = ba.getBuffer();
17921 final var biasX = ba.getElementAtIndex(0);
17922 final var biasY = ba.getElementAtIndex(1);
17923 final var biasZ = ba.getElementAtIndex(2);
17924
17925 final var ma = generateMaCommonAxis();
17926 final var sx = ma.getElementAt(0, 0);
17927 final var sy = ma.getElementAt(1, 1);
17928 final var sz = ma.getElementAt(2, 2);
17929 final var mxy = ma.getElementAt(0, 1);
17930 final var mxz = ma.getElementAt(0, 2);
17931 final var myx = ma.getElementAt(1, 0);
17932 final var myz = ma.getElementAt(1, 2);
17933 final var mzx = ma.getElementAt(2, 0);
17934 final var mzy = ma.getElementAt(2, 1);
17935
17936 final var randomizer = new UniformRandomizer();
17937 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17938 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17939 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17940 final var nedPosition = new NEDPosition(latitude, longitude, height);
17941 final var nedVelocity = new NEDVelocity();
17942 final var ecefPosition = new ECEFPosition();
17943 final var ecefVelocity = new ECEFVelocity();
17944 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
17945 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17946 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17947 final var gravityNorm = gravity.getNorm();
17948
17949 var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
17950 true, ba, ma, this);
17951
17952
17953 assertEquals(biasX, calibrator.getBiasX(), 0.0);
17954 assertEquals(biasY, calibrator.getBiasY(), 0.0);
17955 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
17956 final var bx1 = calibrator.getBiasXAsAcceleration();
17957 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
17958 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
17959 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17960 calibrator.getBiasXAsAcceleration(bx2);
17961 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
17962 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
17963 final var by1 = calibrator.getBiasYAsAcceleration();
17964 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
17965 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
17966 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17967 calibrator.getBiasYAsAcceleration(by2);
17968 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
17969 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
17970 final var bz1 = calibrator.getBiasZAsAcceleration();
17971 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
17972 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
17973 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17974 calibrator.getBiasZAsAcceleration(bz2);
17975 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
17976 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
17977 final var biasTriad1 = calibrator.getBiasAsTriad();
17978 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
17979 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
17980 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
17981 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
17982 final var biasTriad2 = new AccelerationTriad();
17983 calibrator.getBiasAsTriad(biasTriad2);
17984 assertEquals(biasTriad1, biasTriad2);
17985 assertEquals(sx, calibrator.getInitialSx(), 0.0);
17986 assertEquals(sy, calibrator.getInitialSy(), 0.0);
17987 assertEquals(sz, calibrator.getInitialSz(), 0.0);
17988 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
17989 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
17990 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
17991 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
17992 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
17993 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
17994 final var bias1 = calibrator.getBias();
17995 assertArrayEquals(bias1, bias, 0.0);
17996 final var bias2 = new double[3];
17997 calibrator.getBias(bias2);
17998 assertArrayEquals(bias1, bias2, 0.0);
17999 final var b1 = calibrator.getBiasAsMatrix();
18000 assertEquals(b1, ba);
18001 final var b2 = new Matrix(3, 1);
18002 calibrator.getBiasAsMatrix(b2);
18003 assertEquals(b1, b2);
18004 final var ma1 = new Matrix(3, 3);
18005 ma1.setSubmatrix(0, 0, 2, 2,
18006 new double[]{sx, myx, mzx,
18007 mxy, sy, mzy,
18008 mxz, myz, sz});
18009 assertEquals(ma1, calibrator.getInitialMa());
18010 final var ma2 = new Matrix(3, 3);
18011 calibrator.getInitialMa(ma2);
18012 assertEquals(ma1, ma2);
18013 assertSame(measurements, calibrator.getMeasurements());
18014 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18015 calibrator.getMeasurementType());
18016 assertFalse(calibrator.isOrderedMeasurementsRequired());
18017 assertFalse(calibrator.isQualityScoresRequired());
18018 assertTrue(calibrator.isCommonAxisUsed());
18019 assertSame(this, calibrator.getListener());
18020 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
18021 assertFalse(calibrator.isReady());
18022 assertFalse(calibrator.isRunning());
18023 assertNull(calibrator.getEstimatedMa());
18024 assertNull(calibrator.getEstimatedSx());
18025 assertNull(calibrator.getEstimatedSy());
18026 assertNull(calibrator.getEstimatedSz());
18027 assertNull(calibrator.getEstimatedMxy());
18028 assertNull(calibrator.getEstimatedMxz());
18029 assertNull(calibrator.getEstimatedMyx());
18030 assertNull(calibrator.getEstimatedMyz());
18031 assertNull(calibrator.getEstimatedMzx());
18032 assertNull(calibrator.getEstimatedMzy());
18033 assertNull(calibrator.getEstimatedCovariance());
18034 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18035 assertNotNull(calibrator.getGroundTruthGravityNorm());
18036 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18037 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18038 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18039 ABSOLUTE_ERROR));
18040 final var g = new Acceleration(0.0, AccelerationUnit.G);
18041 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18042 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18043 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18044
18045
18046 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18047 -gravityNorm, measurements, true, ba, ma, this));
18048 final var m1 = new Matrix(1, 1);
18049 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18050 gravityNorm, measurements, true, m1, ma, this));
18051 final var m2 = new Matrix(1, 3);
18052 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18053 gravityNorm, measurements, true, m2, ma, this));
18054 final var m3 = new Matrix(1, 3);
18055 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18056 gravityNorm, measurements, true, ba, m3, this));
18057 final var m4 = new Matrix(3, 1);
18058 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18059 gravityNorm, measurements, true, ba, m4, this));
18060 }
18061
18062 @Test
18063 void testConstructor157() throws WrongSizeException {
18064 final var randomizer = new UniformRandomizer();
18065 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18066 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18067 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18068 final var nedPosition = new NEDPosition(latitude, longitude, height);
18069 final var nedVelocity = new NEDVelocity();
18070 final var ecefPosition = new ECEFPosition();
18071 final var ecefVelocity = new ECEFVelocity();
18072 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18073 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18074 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18075 final var gravityNorm = gravity.getNormAsAcceleration();
18076
18077 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm);
18078
18079
18080 assertEquals(0.0, calibrator.getBiasX(), 0.0);
18081 assertEquals(0.0, calibrator.getBiasY(), 0.0);
18082 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18083 final var bx1 = calibrator.getBiasXAsAcceleration();
18084 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18085 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18086 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18087 calibrator.getBiasXAsAcceleration(bx2);
18088 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18089 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18090 final var by1 = calibrator.getBiasYAsAcceleration();
18091 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18092 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18093 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18094 calibrator.getBiasYAsAcceleration(by2);
18095 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18096 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18097 final var bz1 = calibrator.getBiasZAsAcceleration();
18098 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18099 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18100 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18101 calibrator.getBiasZAsAcceleration(bz2);
18102 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18103 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18104 final var biasTriad1 = calibrator.getBiasAsTriad();
18105 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18106 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18107 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18108 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18109 final var biasTriad2 = new AccelerationTriad();
18110 calibrator.getBiasAsTriad(biasTriad2);
18111 assertEquals(biasTriad1, biasTriad2);
18112 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18113 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18114 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18115 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18116 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18117 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18118 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18119 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18120 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18121 final var bias1 = calibrator.getBias();
18122 assertArrayEquals(new double[3], bias1, 0.0);
18123 final var bias2 = new double[3];
18124 calibrator.getBias(bias2);
18125 assertArrayEquals(bias1, bias2, 0.0);
18126 final var b1 = calibrator.getBiasAsMatrix();
18127 assertEquals(new Matrix(3, 1), b1);
18128 final var b2 = new Matrix(3, 1);
18129 calibrator.getBiasAsMatrix(b2);
18130 assertEquals(b1, b2);
18131 final var ma1 = calibrator.getInitialMa();
18132 assertEquals(new Matrix(3, 3), ma1);
18133 final var ma2 = new Matrix(3, 3);
18134 calibrator.getInitialMa(ma2);
18135 assertEquals(ma1, ma2);
18136 assertNull(calibrator.getMeasurements());
18137 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18138 calibrator.getMeasurementType());
18139 assertFalse(calibrator.isOrderedMeasurementsRequired());
18140 assertFalse(calibrator.isQualityScoresRequired());
18141 assertFalse(calibrator.isCommonAxisUsed());
18142 assertNull(calibrator.getListener());
18143 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
18144 assertFalse(calibrator.isReady());
18145 assertFalse(calibrator.isRunning());
18146 assertNull(calibrator.getEstimatedMa());
18147 assertNull(calibrator.getEstimatedSx());
18148 assertNull(calibrator.getEstimatedSy());
18149 assertNull(calibrator.getEstimatedSz());
18150 assertNull(calibrator.getEstimatedMxy());
18151 assertNull(calibrator.getEstimatedMxz());
18152 assertNull(calibrator.getEstimatedMyx());
18153 assertNull(calibrator.getEstimatedMyz());
18154 assertNull(calibrator.getEstimatedMzx());
18155 assertNull(calibrator.getEstimatedMzy());
18156 assertNull(calibrator.getEstimatedCovariance());
18157 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18158 assertNotNull(calibrator.getGroundTruthGravityNorm());
18159 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18160 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18161 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18162 ABSOLUTE_ERROR));
18163 final var g = new Acceleration(0.0, AccelerationUnit.G);
18164 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18165 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18166 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18167
18168
18169 final var invalidGravityNorm = new Acceleration(
18170 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18171
18172 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18173 invalidGravityNorm));
18174 }
18175
18176 @Test
18177 void testConstructor158() throws WrongSizeException {
18178 final var randomizer = new UniformRandomizer();
18179 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18180 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18181 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18182 final var nedPosition = new NEDPosition(latitude, longitude, height);
18183 final var nedVelocity = new NEDVelocity();
18184 final var ecefPosition = new ECEFPosition();
18185 final var ecefVelocity = new ECEFVelocity();
18186 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18187 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18188 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18189 final var gravityNorm = gravity.getNormAsAcceleration();
18190
18191 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, this);
18192
18193
18194 assertEquals(0.0, calibrator.getBiasX(), 0.0);
18195 assertEquals(0.0, calibrator.getBiasY(), 0.0);
18196 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18197 final var bx1 = calibrator.getBiasXAsAcceleration();
18198 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18199 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18200 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18201 calibrator.getBiasXAsAcceleration(bx2);
18202 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18203 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18204 final var by1 = calibrator.getBiasYAsAcceleration();
18205 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18206 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18207 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18208 calibrator.getBiasYAsAcceleration(by2);
18209 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18210 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18211 final var bz1 = calibrator.getBiasZAsAcceleration();
18212 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18213 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18214 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18215 calibrator.getBiasZAsAcceleration(bz2);
18216 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18217 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18218 final var biasTriad1 = calibrator.getBiasAsTriad();
18219 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18220 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18221 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18222 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18223 final var biasTriad2 = new AccelerationTriad();
18224 calibrator.getBiasAsTriad(biasTriad2);
18225 assertEquals(biasTriad1, biasTriad2);
18226 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18227 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18228 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18229 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18230 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18231 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18232 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18233 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18234 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18235 final var bias1 = calibrator.getBias();
18236 assertArrayEquals(new double[3], bias1, 0.0);
18237 final var bias2 = new double[3];
18238 calibrator.getBias(bias2);
18239 assertArrayEquals(bias1, bias2, 0.0);
18240 final var b1 = calibrator.getBiasAsMatrix();
18241 assertEquals(new Matrix(3, 1), b1);
18242 final var b2 = new Matrix(3, 1);
18243 calibrator.getBiasAsMatrix(b2);
18244 assertEquals(b1, b2);
18245 final var ma1 = calibrator.getInitialMa();
18246 assertEquals(new Matrix(3, 3), ma1);
18247 final var ma2 = new Matrix(3, 3);
18248 calibrator.getInitialMa(ma2);
18249 assertEquals(ma1, ma2);
18250 assertNull(calibrator.getMeasurements());
18251 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18252 calibrator.getMeasurementType());
18253 assertFalse(calibrator.isOrderedMeasurementsRequired());
18254 assertFalse(calibrator.isQualityScoresRequired());
18255 assertFalse(calibrator.isCommonAxisUsed());
18256 assertSame(this, calibrator.getListener());
18257 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
18258 assertFalse(calibrator.isReady());
18259 assertFalse(calibrator.isRunning());
18260 assertNull(calibrator.getEstimatedMa());
18261 assertNull(calibrator.getEstimatedSx());
18262 assertNull(calibrator.getEstimatedSy());
18263 assertNull(calibrator.getEstimatedSz());
18264 assertNull(calibrator.getEstimatedMxy());
18265 assertNull(calibrator.getEstimatedMxz());
18266 assertNull(calibrator.getEstimatedMyx());
18267 assertNull(calibrator.getEstimatedMyz());
18268 assertNull(calibrator.getEstimatedMzx());
18269 assertNull(calibrator.getEstimatedMzy());
18270 assertNull(calibrator.getEstimatedCovariance());
18271 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18272 assertNotNull(calibrator.getGroundTruthGravityNorm());
18273 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18274 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18275 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18276 ABSOLUTE_ERROR));
18277 final var g = new Acceleration(0.0, AccelerationUnit.G);
18278 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18279 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18280 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18281
18282
18283 final var invalidGravityNorm = new Acceleration(
18284 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18285
18286 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18287 invalidGravityNorm, this));
18288 }
18289
18290 @Test
18291 void testConstructor159() throws WrongSizeException {
18292 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
18293
18294 final var randomizer = new UniformRandomizer();
18295 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18296 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18297 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18298 final var nedPosition = new NEDPosition(latitude, longitude, height);
18299 final var nedVelocity = new NEDVelocity();
18300 final var ecefPosition = new ECEFPosition();
18301 final var ecefVelocity = new ECEFVelocity();
18302 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18303 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18304 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18305 final var gravityNorm = gravity.getNormAsAcceleration();
18306
18307 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements);
18308
18309
18310 assertEquals(0.0, calibrator.getBiasX(), 0.0);
18311 assertEquals(0.0, calibrator.getBiasY(), 0.0);
18312 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18313 final var bx1 = calibrator.getBiasXAsAcceleration();
18314 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18315 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18316 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18317 calibrator.getBiasXAsAcceleration(bx2);
18318 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18319 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18320 final var by1 = calibrator.getBiasYAsAcceleration();
18321 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18322 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18323 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18324 calibrator.getBiasYAsAcceleration(by2);
18325 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18326 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18327 final var bz1 = calibrator.getBiasZAsAcceleration();
18328 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18329 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18330 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18331 calibrator.getBiasZAsAcceleration(bz2);
18332 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18333 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18334 final var biasTriad1 = calibrator.getBiasAsTriad();
18335 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18336 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18337 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18338 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18339 final var biasTriad2 = new AccelerationTriad();
18340 calibrator.getBiasAsTriad(biasTriad2);
18341 assertEquals(biasTriad1, biasTriad2);
18342 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18343 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18344 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18345 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18346 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18347 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18348 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18349 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18350 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18351 final var bias1 = calibrator.getBias();
18352 assertArrayEquals(new double[3], bias1, 0.0);
18353 final var bias2 = new double[3];
18354 calibrator.getBias(bias2);
18355 assertArrayEquals(bias1, bias2, 0.0);
18356 final var b1 = calibrator.getBiasAsMatrix();
18357 assertEquals(new Matrix(3, 1), b1);
18358 final var b2 = new Matrix(3, 1);
18359 calibrator.getBiasAsMatrix(b2);
18360 assertEquals(b1, b2);
18361 final var ma1 = calibrator.getInitialMa();
18362 assertEquals(new Matrix(3, 3), ma1);
18363 final var ma2 = new Matrix(3, 3);
18364 calibrator.getInitialMa(ma2);
18365 assertEquals(ma1, ma2);
18366 assertSame(measurements, calibrator.getMeasurements());
18367 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18368 calibrator.getMeasurementType());
18369 assertFalse(calibrator.isOrderedMeasurementsRequired());
18370 assertFalse(calibrator.isQualityScoresRequired());
18371 assertFalse(calibrator.isCommonAxisUsed());
18372 assertNull(calibrator.getListener());
18373 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
18374 assertFalse(calibrator.isReady());
18375 assertFalse(calibrator.isRunning());
18376 assertNull(calibrator.getEstimatedMa());
18377 assertNull(calibrator.getEstimatedSx());
18378 assertNull(calibrator.getEstimatedSy());
18379 assertNull(calibrator.getEstimatedSz());
18380 assertNull(calibrator.getEstimatedMxy());
18381 assertNull(calibrator.getEstimatedMxz());
18382 assertNull(calibrator.getEstimatedMyx());
18383 assertNull(calibrator.getEstimatedMyz());
18384 assertNull(calibrator.getEstimatedMzx());
18385 assertNull(calibrator.getEstimatedMzy());
18386 assertNull(calibrator.getEstimatedCovariance());
18387 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18388 assertNotNull(calibrator.getGroundTruthGravityNorm());
18389 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18390 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18391 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18392 ABSOLUTE_ERROR));
18393 final var g = new Acceleration(0.0, AccelerationUnit.G);
18394 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18395 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18396 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18397
18398
18399 final var invalidGravityNorm = new Acceleration(
18400 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18401
18402 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18403 invalidGravityNorm, measurements));
18404 }
18405
18406 @Test
18407 void testConstructor160() throws WrongSizeException {
18408 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
18409
18410 final var randomizer = new UniformRandomizer();
18411 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18412 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18413 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18414 final var nedPosition = new NEDPosition(latitude, longitude, height);
18415 final var nedVelocity = new NEDVelocity();
18416 final var ecefPosition = new ECEFPosition();
18417 final var ecefVelocity = new ECEFVelocity();
18418 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18419 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18420 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18421 final var gravityNorm = gravity.getNormAsAcceleration();
18422
18423 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
18424 this);
18425
18426
18427 assertEquals(0.0, calibrator.getBiasX(), 0.0);
18428 assertEquals(0.0, calibrator.getBiasY(), 0.0);
18429 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18430 final var bx1 = calibrator.getBiasXAsAcceleration();
18431 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18432 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18433 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18434 calibrator.getBiasXAsAcceleration(bx2);
18435 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18436 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18437 final var by1 = calibrator.getBiasYAsAcceleration();
18438 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18439 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18440 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18441 calibrator.getBiasYAsAcceleration(by2);
18442 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18443 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18444 final var bz1 = calibrator.getBiasZAsAcceleration();
18445 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18446 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18447 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18448 calibrator.getBiasZAsAcceleration(bz2);
18449 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18450 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18451 final var biasTriad1 = calibrator.getBiasAsTriad();
18452 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18453 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18454 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18455 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18456 final var biasTriad2 = new AccelerationTriad();
18457 calibrator.getBiasAsTriad(biasTriad2);
18458 assertEquals(biasTriad1, biasTriad2);
18459 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18460 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18461 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18462 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18463 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18464 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18465 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18466 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18467 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18468 final var bias1 = calibrator.getBias();
18469 assertArrayEquals(new double[3], bias1, 0.0);
18470 final var bias2 = new double[3];
18471 calibrator.getBias(bias2);
18472 assertArrayEquals(bias1, bias2, 0.0);
18473 final var b1 = calibrator.getBiasAsMatrix();
18474 assertEquals(new Matrix(3, 1), b1);
18475 final var b2 = new Matrix(3, 1);
18476 calibrator.getBiasAsMatrix(b2);
18477 assertEquals(b1, b2);
18478 final var ma1 = calibrator.getInitialMa();
18479 assertEquals(new Matrix(3, 3), ma1);
18480 final var ma2 = new Matrix(3, 3);
18481 calibrator.getInitialMa(ma2);
18482 assertEquals(ma1, ma2);
18483 assertSame(measurements, calibrator.getMeasurements());
18484 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18485 calibrator.getMeasurementType());
18486 assertFalse(calibrator.isOrderedMeasurementsRequired());
18487 assertFalse(calibrator.isQualityScoresRequired());
18488 assertFalse(calibrator.isCommonAxisUsed());
18489 assertSame(this, calibrator.getListener());
18490 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
18491 assertFalse(calibrator.isReady());
18492 assertFalse(calibrator.isRunning());
18493 assertNull(calibrator.getEstimatedMa());
18494 assertNull(calibrator.getEstimatedSx());
18495 assertNull(calibrator.getEstimatedSy());
18496 assertNull(calibrator.getEstimatedSz());
18497 assertNull(calibrator.getEstimatedMxy());
18498 assertNull(calibrator.getEstimatedMxz());
18499 assertNull(calibrator.getEstimatedMyx());
18500 assertNull(calibrator.getEstimatedMyz());
18501 assertNull(calibrator.getEstimatedMzx());
18502 assertNull(calibrator.getEstimatedMzy());
18503 assertNull(calibrator.getEstimatedCovariance());
18504 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18505 assertNotNull(calibrator.getGroundTruthGravityNorm());
18506 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18507 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18508 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18509 ABSOLUTE_ERROR));
18510 final var g = new Acceleration(0.0, AccelerationUnit.G);
18511 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18512 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18513 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18514
18515
18516 final var invalidGravityNorm = new Acceleration(
18517 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18518
18519 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18520 invalidGravityNorm, measurements, this));
18521 }
18522
18523 @Test
18524 void testConstructor161() throws WrongSizeException {
18525 final var randomizer = new UniformRandomizer();
18526 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18527 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18528 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18529 final var nedPosition = new NEDPosition(latitude, longitude, height);
18530 final var nedVelocity = new NEDVelocity();
18531 final var ecefPosition = new ECEFPosition();
18532 final var ecefVelocity = new ECEFVelocity();
18533 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18534 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18535 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18536 final var gravityNorm = gravity.getNormAsAcceleration();
18537
18538 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true);
18539
18540
18541 assertEquals(0.0, calibrator.getBiasX(), 0.0);
18542 assertEquals(0.0, calibrator.getBiasY(), 0.0);
18543 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18544 final var bx1 = calibrator.getBiasXAsAcceleration();
18545 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18546 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18547 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18548 calibrator.getBiasXAsAcceleration(bx2);
18549 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18550 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18551 final var by1 = calibrator.getBiasYAsAcceleration();
18552 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18553 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18554 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18555 calibrator.getBiasYAsAcceleration(by2);
18556 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18557 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18558 final var bz1 = calibrator.getBiasZAsAcceleration();
18559 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18560 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18561 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18562 calibrator.getBiasZAsAcceleration(bz2);
18563 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18564 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18565 final var biasTriad1 = calibrator.getBiasAsTriad();
18566 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18567 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18568 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18569 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18570 final var biasTriad2 = new AccelerationTriad();
18571 calibrator.getBiasAsTriad(biasTriad2);
18572 assertEquals(biasTriad1, biasTriad2);
18573 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18574 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18575 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18576 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18577 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18578 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18579 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18580 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18581 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18582 final var bias1 = calibrator.getBias();
18583 assertArrayEquals(new double[3], bias1, 0.0);
18584 final var bias2 = new double[3];
18585 calibrator.getBias(bias2);
18586 assertArrayEquals(bias1, bias2, 0.0);
18587 final var b1 = calibrator.getBiasAsMatrix();
18588 assertEquals(new Matrix(3, 1), b1);
18589 final var b2 = new Matrix(3, 1);
18590 calibrator.getBiasAsMatrix(b2);
18591 assertEquals(b1, b2);
18592 final var ma1 = calibrator.getInitialMa();
18593 assertEquals(new Matrix(3, 3), ma1);
18594 final var ma2 = new Matrix(3, 3);
18595 calibrator.getInitialMa(ma2);
18596 assertEquals(ma1, ma2);
18597 assertNull(calibrator.getMeasurements());
18598 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18599 calibrator.getMeasurementType());
18600 assertFalse(calibrator.isOrderedMeasurementsRequired());
18601 assertFalse(calibrator.isQualityScoresRequired());
18602 assertTrue(calibrator.isCommonAxisUsed());
18603 assertNull(calibrator.getListener());
18604 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
18605 assertFalse(calibrator.isReady());
18606 assertFalse(calibrator.isRunning());
18607 assertNull(calibrator.getEstimatedMa());
18608 assertNull(calibrator.getEstimatedSx());
18609 assertNull(calibrator.getEstimatedSy());
18610 assertNull(calibrator.getEstimatedSz());
18611 assertNull(calibrator.getEstimatedMxy());
18612 assertNull(calibrator.getEstimatedMxz());
18613 assertNull(calibrator.getEstimatedMyx());
18614 assertNull(calibrator.getEstimatedMyz());
18615 assertNull(calibrator.getEstimatedMzx());
18616 assertNull(calibrator.getEstimatedMzy());
18617 assertNull(calibrator.getEstimatedCovariance());
18618 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18619 assertNotNull(calibrator.getGroundTruthGravityNorm());
18620 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18621 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18622 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18623 ABSOLUTE_ERROR));
18624 final var g = new Acceleration(0.0, AccelerationUnit.G);
18625 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18626 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18627 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18628
18629
18630 final var invalidGravityNorm = new Acceleration(
18631 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18632
18633 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18634 invalidGravityNorm, true));
18635 }
18636
18637 @Test
18638 void testConstructor162() throws WrongSizeException {
18639 final var randomizer = new UniformRandomizer();
18640 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18641 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18642 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18643 final var nedPosition = new NEDPosition(latitude, longitude, height);
18644 final var nedVelocity = new NEDVelocity();
18645 final var ecefPosition = new ECEFPosition();
18646 final var ecefVelocity = new ECEFVelocity();
18647 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18648 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18649 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18650 final var gravityNorm = gravity.getNormAsAcceleration();
18651
18652 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
18653 this);
18654
18655
18656 assertEquals(0.0, calibrator.getBiasX(), 0.0);
18657 assertEquals(0.0, calibrator.getBiasY(), 0.0);
18658 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18659 final var bx1 = calibrator.getBiasXAsAcceleration();
18660 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18661 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18662 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18663 calibrator.getBiasXAsAcceleration(bx2);
18664 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18665 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18666 final var by1 = calibrator.getBiasYAsAcceleration();
18667 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18668 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18669 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18670 calibrator.getBiasYAsAcceleration(by2);
18671 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18672 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18673 final var bz1 = calibrator.getBiasZAsAcceleration();
18674 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18675 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18676 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18677 calibrator.getBiasZAsAcceleration(bz2);
18678 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18679 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18680 final var biasTriad1 = calibrator.getBiasAsTriad();
18681 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18682 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18683 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18684 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18685 final var biasTriad2 = new AccelerationTriad();
18686 calibrator.getBiasAsTriad(biasTriad2);
18687 assertEquals(biasTriad1, biasTriad2);
18688 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18689 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18690 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18691 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18692 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18693 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18694 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18695 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18696 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18697 final var bias1 = calibrator.getBias();
18698 assertArrayEquals(new double[3], bias1, 0.0);
18699 final var bias2 = new double[3];
18700 calibrator.getBias(bias2);
18701 assertArrayEquals(bias1, bias2, 0.0);
18702 final var b1 = calibrator.getBiasAsMatrix();
18703 assertEquals(new Matrix(3, 1), b1);
18704 final var b2 = new Matrix(3, 1);
18705 calibrator.getBiasAsMatrix(b2);
18706 assertEquals(b1, b2);
18707 final var ma1 = calibrator.getInitialMa();
18708 assertEquals(new Matrix(3, 3), ma1);
18709 final var ma2 = new Matrix(3, 3);
18710 calibrator.getInitialMa(ma2);
18711 assertEquals(ma1, ma2);
18712 assertNull(calibrator.getMeasurements());
18713 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18714 calibrator.getMeasurementType());
18715 assertFalse(calibrator.isOrderedMeasurementsRequired());
18716 assertFalse(calibrator.isQualityScoresRequired());
18717 assertTrue(calibrator.isCommonAxisUsed());
18718 assertSame(this, calibrator.getListener());
18719 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
18720 assertFalse(calibrator.isReady());
18721 assertFalse(calibrator.isRunning());
18722 assertNull(calibrator.getEstimatedMa());
18723 assertNull(calibrator.getEstimatedSx());
18724 assertNull(calibrator.getEstimatedSy());
18725 assertNull(calibrator.getEstimatedSz());
18726 assertNull(calibrator.getEstimatedMxy());
18727 assertNull(calibrator.getEstimatedMxz());
18728 assertNull(calibrator.getEstimatedMyx());
18729 assertNull(calibrator.getEstimatedMyz());
18730 assertNull(calibrator.getEstimatedMzx());
18731 assertNull(calibrator.getEstimatedMzy());
18732 assertNull(calibrator.getEstimatedCovariance());
18733 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18734 assertNotNull(calibrator.getGroundTruthGravityNorm());
18735 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18736 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18737 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18738 ABSOLUTE_ERROR));
18739 final var g = new Acceleration(0.0, AccelerationUnit.G);
18740 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18741 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18742 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18743
18744
18745 final var invalidGravityNorm = new Acceleration(
18746 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18747
18748 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18749 invalidGravityNorm, true, this));
18750 }
18751
18752 @Test
18753 void testConstructor163() throws WrongSizeException {
18754 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
18755 final var randomizer = new UniformRandomizer();
18756 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18757 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18758 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18759 final var nedPosition = new NEDPosition(latitude, longitude, height);
18760 final var nedVelocity = new NEDVelocity();
18761 final var ecefPosition = new ECEFPosition();
18762 final var ecefVelocity = new ECEFVelocity();
18763 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18764 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18765 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18766 final var gravityNorm = gravity.getNormAsAcceleration();
18767
18768 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
18769 true);
18770
18771
18772 assertEquals(0.0, calibrator.getBiasX(), 0.0);
18773 assertEquals(0.0, calibrator.getBiasY(), 0.0);
18774 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18775 final var bx1 = calibrator.getBiasXAsAcceleration();
18776 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18777 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18778 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18779 calibrator.getBiasXAsAcceleration(bx2);
18780 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18781 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18782 final var by1 = calibrator.getBiasYAsAcceleration();
18783 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18784 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18785 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18786 calibrator.getBiasYAsAcceleration(by2);
18787 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18788 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18789 final var bz1 = calibrator.getBiasZAsAcceleration();
18790 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18791 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18792 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18793 calibrator.getBiasZAsAcceleration(bz2);
18794 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18795 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18796 final var biasTriad1 = calibrator.getBiasAsTriad();
18797 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18798 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18799 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18800 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18801 final var biasTriad2 = new AccelerationTriad();
18802 calibrator.getBiasAsTriad(biasTriad2);
18803 assertEquals(biasTriad1, biasTriad2);
18804 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18805 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18806 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18807 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18808 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18809 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18810 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18811 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18812 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18813 final var bias1 = calibrator.getBias();
18814 assertArrayEquals(new double[3], bias1, 0.0);
18815 final var bias2 = new double[3];
18816 calibrator.getBias(bias2);
18817 assertArrayEquals(bias1, bias2, 0.0);
18818 final var b1 = calibrator.getBiasAsMatrix();
18819 assertEquals(new Matrix(3, 1), b1);
18820 final var b2 = new Matrix(3, 1);
18821 calibrator.getBiasAsMatrix(b2);
18822 assertEquals(b1, b2);
18823 final var ma1 = calibrator.getInitialMa();
18824 assertEquals(new Matrix(3, 3), ma1);
18825 final var ma2 = new Matrix(3, 3);
18826 calibrator.getInitialMa(ma2);
18827 assertEquals(ma1, ma2);
18828 assertSame(measurements, calibrator.getMeasurements());
18829 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18830 calibrator.getMeasurementType());
18831 assertFalse(calibrator.isOrderedMeasurementsRequired());
18832 assertFalse(calibrator.isQualityScoresRequired());
18833 assertTrue(calibrator.isCommonAxisUsed());
18834 assertNull(calibrator.getListener());
18835 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
18836 assertFalse(calibrator.isReady());
18837 assertFalse(calibrator.isRunning());
18838 assertNull(calibrator.getEstimatedMa());
18839 assertNull(calibrator.getEstimatedSx());
18840 assertNull(calibrator.getEstimatedSy());
18841 assertNull(calibrator.getEstimatedSz());
18842 assertNull(calibrator.getEstimatedMxy());
18843 assertNull(calibrator.getEstimatedMxz());
18844 assertNull(calibrator.getEstimatedMyx());
18845 assertNull(calibrator.getEstimatedMyz());
18846 assertNull(calibrator.getEstimatedMzx());
18847 assertNull(calibrator.getEstimatedMzy());
18848 assertNull(calibrator.getEstimatedCovariance());
18849 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18850 assertNotNull(calibrator.getGroundTruthGravityNorm());
18851 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18852 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18853 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18854 ABSOLUTE_ERROR));
18855 final var g = new Acceleration(0.0, AccelerationUnit.G);
18856 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18857 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18858 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18859
18860
18861 final var invalidGravityNorm = new Acceleration(
18862 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18863
18864 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18865 invalidGravityNorm, measurements, true));
18866 }
18867
18868 @Test
18869 void testConstructor164() throws WrongSizeException {
18870 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
18871 final var randomizer = new UniformRandomizer();
18872 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18873 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18874 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18875 final var nedPosition = new NEDPosition(latitude, longitude, height);
18876 final var nedVelocity = new NEDVelocity();
18877 final var ecefPosition = new ECEFPosition();
18878 final var ecefVelocity = new ECEFVelocity();
18879 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18880 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18881 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18882 final var gravityNorm = gravity.getNormAsAcceleration();
18883
18884 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
18885 true, this);
18886
18887
18888 assertEquals(0.0, calibrator.getBiasX(), 0.0);
18889 assertEquals(0.0, calibrator.getBiasY(), 0.0);
18890 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18891 final var bx1 = calibrator.getBiasXAsAcceleration();
18892 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18893 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18894 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18895 calibrator.getBiasXAsAcceleration(bx2);
18896 assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18897 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18898 final var by1 = calibrator.getBiasYAsAcceleration();
18899 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18900 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18901 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18902 calibrator.getBiasYAsAcceleration(by2);
18903 assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18904 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18905 final var bz1 = calibrator.getBiasZAsAcceleration();
18906 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18907 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18908 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18909 calibrator.getBiasZAsAcceleration(bz2);
18910 assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18911 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18912 final var biasTriad1 = calibrator.getBiasAsTriad();
18913 assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18914 assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18915 assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18916 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18917 final var biasTriad2 = new AccelerationTriad();
18918 calibrator.getBiasAsTriad(biasTriad2);
18919 assertEquals(biasTriad1, biasTriad2);
18920 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18921 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18922 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18923 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18924 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18925 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18926 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18927 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18928 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18929 final var bias1 = calibrator.getBias();
18930 assertArrayEquals(new double[3], bias1, 0.0);
18931 final var bias2 = new double[3];
18932 calibrator.getBias(bias2);
18933 assertArrayEquals(bias1, bias2, 0.0);
18934 final var b1 = calibrator.getBiasAsMatrix();
18935 assertEquals(new Matrix(3, 1), b1);
18936 final var b2 = new Matrix(3, 1);
18937 calibrator.getBiasAsMatrix(b2);
18938 assertEquals(b1, b2);
18939 final var ma1 = calibrator.getInitialMa();
18940 assertEquals(new Matrix(3, 3), ma1);
18941 final var ma2 = new Matrix(3, 3);
18942 calibrator.getInitialMa(ma2);
18943 assertEquals(ma1, ma2);
18944 assertSame(measurements, calibrator.getMeasurements());
18945 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18946 calibrator.getMeasurementType());
18947 assertFalse(calibrator.isOrderedMeasurementsRequired());
18948 assertFalse(calibrator.isQualityScoresRequired());
18949 assertTrue(calibrator.isCommonAxisUsed());
18950 assertSame(this, calibrator.getListener());
18951 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
18952 assertFalse(calibrator.isReady());
18953 assertFalse(calibrator.isRunning());
18954 assertNull(calibrator.getEstimatedMa());
18955 assertNull(calibrator.getEstimatedSx());
18956 assertNull(calibrator.getEstimatedSy());
18957 assertNull(calibrator.getEstimatedSz());
18958 assertNull(calibrator.getEstimatedMxy());
18959 assertNull(calibrator.getEstimatedMxz());
18960 assertNull(calibrator.getEstimatedMyx());
18961 assertNull(calibrator.getEstimatedMyz());
18962 assertNull(calibrator.getEstimatedMzx());
18963 assertNull(calibrator.getEstimatedMzy());
18964 assertNull(calibrator.getEstimatedCovariance());
18965 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18966 assertNotNull(calibrator.getGroundTruthGravityNorm());
18967 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18968 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18969 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18970 ABSOLUTE_ERROR));
18971 final var g = new Acceleration(0.0, AccelerationUnit.G);
18972 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18973 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18974 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18975
18976
18977 final var invalidGravityNorm = new Acceleration(
18978 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18979
18980 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18981 invalidGravityNorm, measurements, true, this));
18982 }
18983
18984 @Test
18985 void testConstructor165() throws WrongSizeException {
18986 final var ba = generateBa();
18987 final var biasX = ba.getElementAtIndex(0);
18988 final var biasY = ba.getElementAtIndex(1);
18989 final var biasZ = ba.getElementAtIndex(2);
18990
18991 final var randomizer = new UniformRandomizer();
18992 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18993 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18994 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18995 final var nedPosition = new NEDPosition(latitude, longitude, height);
18996 final var nedVelocity = new NEDVelocity();
18997 final var ecefPosition = new ECEFPosition();
18998 final var ecefVelocity = new ECEFVelocity();
18999 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19000 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19001 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19002 final var gravityNorm = gravity.getNormAsAcceleration();
19003
19004 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ);
19005
19006
19007 assertEquals(biasX, calibrator.getBiasX(), 0.0);
19008 assertEquals(biasY, calibrator.getBiasY(), 0.0);
19009 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19010 final var bx1 = calibrator.getBiasXAsAcceleration();
19011 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19012 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19013 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19014 calibrator.getBiasXAsAcceleration(bx2);
19015 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19016 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19017 final var by1 = calibrator.getBiasYAsAcceleration();
19018 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19019 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19020 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19021 calibrator.getBiasYAsAcceleration(by2);
19022 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19023 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19024 final var bz1 = calibrator.getBiasZAsAcceleration();
19025 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19026 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19027 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19028 calibrator.getBiasZAsAcceleration(bz2);
19029 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19030 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19031 final var biasTriad1 = calibrator.getBiasAsTriad();
19032 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19033 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19034 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19035 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19036 final var biasTriad2 = new AccelerationTriad();
19037 calibrator.getBiasAsTriad(biasTriad2);
19038 assertEquals(biasTriad1, biasTriad2);
19039 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19040 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19041 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19042 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19043 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19044 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19045 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19046 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19047 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19048 final var bias1 = calibrator.getBias();
19049 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19050 final var bias2 = new double[3];
19051 calibrator.getBias(bias2);
19052 assertArrayEquals(bias1, bias2, 0.0);
19053 final var b1 = calibrator.getBiasAsMatrix();
19054 assertEquals(b1, ba);
19055 final var b2 = new Matrix(3, 1);
19056 calibrator.getBiasAsMatrix(b2);
19057 assertEquals(b1, b2);
19058 final var ma1 = calibrator.getInitialMa();
19059 assertEquals(new Matrix(3, 3), ma1);
19060 final var ma2 = new Matrix(3, 3);
19061 calibrator.getInitialMa(ma2);
19062 assertEquals(ma1, ma2);
19063 assertNull(calibrator.getMeasurements());
19064 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19065 calibrator.getMeasurementType());
19066 assertFalse(calibrator.isOrderedMeasurementsRequired());
19067 assertFalse(calibrator.isQualityScoresRequired());
19068 assertFalse(calibrator.isCommonAxisUsed());
19069 assertNull(calibrator.getListener());
19070 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
19071 assertFalse(calibrator.isReady());
19072 assertFalse(calibrator.isRunning());
19073 assertNull(calibrator.getEstimatedMa());
19074 assertNull(calibrator.getEstimatedSx());
19075 assertNull(calibrator.getEstimatedSy());
19076 assertNull(calibrator.getEstimatedSz());
19077 assertNull(calibrator.getEstimatedMxy());
19078 assertNull(calibrator.getEstimatedMxz());
19079 assertNull(calibrator.getEstimatedMyx());
19080 assertNull(calibrator.getEstimatedMyz());
19081 assertNull(calibrator.getEstimatedMzx());
19082 assertNull(calibrator.getEstimatedMzy());
19083 assertNull(calibrator.getEstimatedCovariance());
19084 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19085 assertNotNull(calibrator.getGroundTruthGravityNorm());
19086 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19087 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19088 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19089 ABSOLUTE_ERROR));
19090 final var g = new Acceleration(0.0, AccelerationUnit.G);
19091 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19092 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19093 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19094
19095
19096 final var invalidGravityNorm = new Acceleration(
19097 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19098
19099 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19100 invalidGravityNorm, biasX, biasY, biasZ));
19101 }
19102
19103 @Test
19104 void testConstructor166() throws WrongSizeException {
19105 final var ba = generateBa();
19106 final var biasX = ba.getElementAtIndex(0);
19107 final var biasY = ba.getElementAtIndex(1);
19108 final var biasZ = ba.getElementAtIndex(2);
19109
19110 final var randomizer = new UniformRandomizer();
19111 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19112 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19113 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19114 final var nedPosition = new NEDPosition(latitude, longitude, height);
19115 final var nedVelocity = new NEDVelocity();
19116 final var ecefPosition = new ECEFPosition();
19117 final var ecefVelocity = new ECEFVelocity();
19118 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19119 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19120 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19121 final var gravityNorm = gravity.getNormAsAcceleration();
19122
19123 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ,
19124 this);
19125
19126
19127 assertEquals(biasX, calibrator.getBiasX(), 0.0);
19128 assertEquals(biasY, calibrator.getBiasY(), 0.0);
19129 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19130 final var bx1 = calibrator.getBiasXAsAcceleration();
19131 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19132 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19133 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19134 calibrator.getBiasXAsAcceleration(bx2);
19135 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19136 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19137 final var by1 = calibrator.getBiasYAsAcceleration();
19138 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19139 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19140 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19141 calibrator.getBiasYAsAcceleration(by2);
19142 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19143 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19144 final var bz1 = calibrator.getBiasZAsAcceleration();
19145 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19146 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19147 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19148 calibrator.getBiasZAsAcceleration(bz2);
19149 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19150 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19151 final var biasTriad1 = calibrator.getBiasAsTriad();
19152 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19153 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19154 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19155 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19156 final var biasTriad2 = new AccelerationTriad();
19157 calibrator.getBiasAsTriad(biasTriad2);
19158 assertEquals(biasTriad1, biasTriad2);
19159 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19160 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19161 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19162 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19163 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19164 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19165 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19166 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19167 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19168 final var bias1 = calibrator.getBias();
19169 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19170 final var bias2 = new double[3];
19171 calibrator.getBias(bias2);
19172 assertArrayEquals(bias1, bias2, 0.0);
19173 final var b1 = calibrator.getBiasAsMatrix();
19174 assertEquals(b1, ba);
19175 final var b2 = new Matrix(3, 1);
19176 calibrator.getBiasAsMatrix(b2);
19177 assertEquals(b1, b2);
19178 final var ma1 = calibrator.getInitialMa();
19179 assertEquals(new Matrix(3, 3), ma1);
19180 final var ma2 = new Matrix(3, 3);
19181 calibrator.getInitialMa(ma2);
19182 assertEquals(ma1, ma2);
19183 assertNull(calibrator.getMeasurements());
19184 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19185 calibrator.getMeasurementType());
19186 assertFalse(calibrator.isOrderedMeasurementsRequired());
19187 assertFalse(calibrator.isQualityScoresRequired());
19188 assertFalse(calibrator.isCommonAxisUsed());
19189 assertSame(this, calibrator.getListener());
19190 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
19191 assertFalse(calibrator.isReady());
19192 assertFalse(calibrator.isRunning());
19193 assertNull(calibrator.getEstimatedMa());
19194 assertNull(calibrator.getEstimatedSx());
19195 assertNull(calibrator.getEstimatedSy());
19196 assertNull(calibrator.getEstimatedSz());
19197 assertNull(calibrator.getEstimatedMxy());
19198 assertNull(calibrator.getEstimatedMxz());
19199 assertNull(calibrator.getEstimatedMyx());
19200 assertNull(calibrator.getEstimatedMyz());
19201 assertNull(calibrator.getEstimatedMzx());
19202 assertNull(calibrator.getEstimatedMzy());
19203 assertNull(calibrator.getEstimatedCovariance());
19204 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19205 assertNotNull(calibrator.getGroundTruthGravityNorm());
19206 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19207 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19208 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19209 ABSOLUTE_ERROR));
19210 final var g = new Acceleration(0.0, AccelerationUnit.G);
19211 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19212 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19213 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19214
19215
19216 final var invalidGravityNorm = new Acceleration(
19217 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19218
19219 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19220 invalidGravityNorm, biasX, biasY, biasZ, this));
19221 }
19222
19223 @Test
19224 void testConstructor167() throws WrongSizeException {
19225 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
19226
19227 final var ba = generateBa();
19228 final var biasX = ba.getElementAtIndex(0);
19229 final var biasY = ba.getElementAtIndex(1);
19230 final var biasZ = ba.getElementAtIndex(2);
19231
19232 final var randomizer = new UniformRandomizer();
19233 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19234 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19235 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19236 final var nedPosition = new NEDPosition(latitude, longitude, height);
19237 final var nedVelocity = new NEDVelocity();
19238 final var ecefPosition = new ECEFPosition();
19239 final var ecefVelocity = new ECEFVelocity();
19240 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19241 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19242 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19243 final var gravityNorm = gravity.getNormAsAcceleration();
19244
19245 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
19246 biasX, biasY, biasZ);
19247
19248
19249 assertEquals(biasX, calibrator.getBiasX(), 0.0);
19250 assertEquals(biasY, calibrator.getBiasY(), 0.0);
19251 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19252 final var bx1 = calibrator.getBiasXAsAcceleration();
19253 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19254 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19255 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19256 calibrator.getBiasXAsAcceleration(bx2);
19257 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19258 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19259 final var by1 = calibrator.getBiasYAsAcceleration();
19260 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19261 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19262 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19263 calibrator.getBiasYAsAcceleration(by2);
19264 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19265 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19266 final var bz1 = calibrator.getBiasZAsAcceleration();
19267 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19268 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19269 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19270 calibrator.getBiasZAsAcceleration(bz2);
19271 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19272 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19273 final var biasTriad1 = calibrator.getBiasAsTriad();
19274 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19275 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19276 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19277 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19278 final var biasTriad2 = new AccelerationTriad();
19279 calibrator.getBiasAsTriad(biasTriad2);
19280 assertEquals(biasTriad1, biasTriad2);
19281 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19282 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19283 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19284 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19285 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19286 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19287 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19288 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19289 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19290 final var bias1 = calibrator.getBias();
19291 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19292 final var bias2 = new double[3];
19293 calibrator.getBias(bias2);
19294 assertArrayEquals(bias1, bias2, 0.0);
19295 final var b1 = calibrator.getBiasAsMatrix();
19296 assertEquals(b1, ba);
19297 final var b2 = new Matrix(3, 1);
19298 calibrator.getBiasAsMatrix(b2);
19299 assertEquals(b1, b2);
19300 final var ma1 = calibrator.getInitialMa();
19301 assertEquals(new Matrix(3, 3), ma1);
19302 final var ma2 = new Matrix(3, 3);
19303 calibrator.getInitialMa(ma2);
19304 assertEquals(ma1, ma2);
19305 assertSame(measurements, calibrator.getMeasurements());
19306 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19307 calibrator.getMeasurementType());
19308 assertFalse(calibrator.isOrderedMeasurementsRequired());
19309 assertFalse(calibrator.isQualityScoresRequired());
19310 assertFalse(calibrator.isCommonAxisUsed());
19311 assertNull(calibrator.getListener());
19312 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
19313 assertFalse(calibrator.isReady());
19314 assertFalse(calibrator.isRunning());
19315 assertNull(calibrator.getEstimatedMa());
19316 assertNull(calibrator.getEstimatedSx());
19317 assertNull(calibrator.getEstimatedSy());
19318 assertNull(calibrator.getEstimatedSz());
19319 assertNull(calibrator.getEstimatedMxy());
19320 assertNull(calibrator.getEstimatedMxz());
19321 assertNull(calibrator.getEstimatedMyx());
19322 assertNull(calibrator.getEstimatedMyz());
19323 assertNull(calibrator.getEstimatedMzx());
19324 assertNull(calibrator.getEstimatedMzy());
19325 assertNull(calibrator.getEstimatedCovariance());
19326 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19327 assertNotNull(calibrator.getGroundTruthGravityNorm());
19328 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19329 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19330 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19331 ABSOLUTE_ERROR));
19332 final var g = new Acceleration(0.0, AccelerationUnit.G);
19333 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19334 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19335 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19336
19337
19338 final var invalidGravityNorm = new Acceleration(
19339 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19340
19341 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19342 invalidGravityNorm, measurements, biasX, biasY, biasZ));
19343 }
19344
19345 @Test
19346 void testConstructor168() throws WrongSizeException {
19347 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
19348
19349 final var ba = generateBa();
19350 final var biasX = ba.getElementAtIndex(0);
19351 final var biasY = ba.getElementAtIndex(1);
19352 final var biasZ = ba.getElementAtIndex(2);
19353
19354 final var randomizer = new UniformRandomizer();
19355 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19356 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19357 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19358 final var nedPosition = new NEDPosition(latitude, longitude, height);
19359 final var nedVelocity = new NEDVelocity();
19360 final var ecefPosition = new ECEFPosition();
19361 final var ecefVelocity = new ECEFVelocity();
19362 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19363 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19364 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19365 final var gravityNorm = gravity.getNormAsAcceleration();
19366
19367 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
19368 biasX, biasY, biasZ, this);
19369
19370
19371 assertEquals(biasX, calibrator.getBiasX(), 0.0);
19372 assertEquals(biasY, calibrator.getBiasY(), 0.0);
19373 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19374 final var bx1 = calibrator.getBiasXAsAcceleration();
19375 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19376 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19377 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19378 calibrator.getBiasXAsAcceleration(bx2);
19379 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19380 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19381 final var by1 = calibrator.getBiasYAsAcceleration();
19382 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19383 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19384 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19385 calibrator.getBiasYAsAcceleration(by2);
19386 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19387 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19388 final var bz1 = calibrator.getBiasZAsAcceleration();
19389 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19390 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19391 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19392 calibrator.getBiasZAsAcceleration(bz2);
19393 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19394 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19395 final var biasTriad1 = calibrator.getBiasAsTriad();
19396 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19397 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19398 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19399 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19400 final var biasTriad2 = new AccelerationTriad();
19401 calibrator.getBiasAsTriad(biasTriad2);
19402 assertEquals(biasTriad1, biasTriad2);
19403 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19404 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19405 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19406 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19407 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19408 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19409 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19410 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19411 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19412 final var bias1 = calibrator.getBias();
19413 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19414 final var bias2 = new double[3];
19415 calibrator.getBias(bias2);
19416 assertArrayEquals(bias1, bias2, 0.0);
19417 final var b1 = calibrator.getBiasAsMatrix();
19418 assertEquals(b1, ba);
19419 final var b2 = new Matrix(3, 1);
19420 calibrator.getBiasAsMatrix(b2);
19421 assertEquals(b1, b2);
19422 final var ma1 = calibrator.getInitialMa();
19423 assertEquals(new Matrix(3, 3), ma1);
19424 final var ma2 = new Matrix(3, 3);
19425 calibrator.getInitialMa(ma2);
19426 assertEquals(ma1, ma2);
19427 assertSame(measurements, calibrator.getMeasurements());
19428 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19429 calibrator.getMeasurementType());
19430 assertFalse(calibrator.isOrderedMeasurementsRequired());
19431 assertFalse(calibrator.isQualityScoresRequired());
19432 assertFalse(calibrator.isCommonAxisUsed());
19433 assertSame(this, calibrator.getListener());
19434 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
19435 assertFalse(calibrator.isReady());
19436 assertFalse(calibrator.isRunning());
19437 assertNull(calibrator.getEstimatedMa());
19438 assertNull(calibrator.getEstimatedSx());
19439 assertNull(calibrator.getEstimatedSy());
19440 assertNull(calibrator.getEstimatedSz());
19441 assertNull(calibrator.getEstimatedMxy());
19442 assertNull(calibrator.getEstimatedMxz());
19443 assertNull(calibrator.getEstimatedMyx());
19444 assertNull(calibrator.getEstimatedMyz());
19445 assertNull(calibrator.getEstimatedMzx());
19446 assertNull(calibrator.getEstimatedMzy());
19447 assertNull(calibrator.getEstimatedCovariance());
19448 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19449 assertNotNull(calibrator.getGroundTruthGravityNorm());
19450 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19451 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19452 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19453 ABSOLUTE_ERROR));
19454 final var g = new Acceleration(0.0, AccelerationUnit.G);
19455 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19456 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19457 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19458
19459
19460 final var invalidGravityNorm = new Acceleration(
19461 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19462
19463 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19464 invalidGravityNorm, measurements, biasX, biasY, biasZ, this));
19465 }
19466
19467 @Test
19468 void testConstructor169() throws WrongSizeException {
19469 final var ba = generateBa();
19470 final var biasX = ba.getElementAtIndex(0);
19471 final var biasY = ba.getElementAtIndex(1);
19472 final var biasZ = ba.getElementAtIndex(2);
19473
19474 final var randomizer = new UniformRandomizer();
19475 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19476 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19477 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19478 final var nedPosition = new NEDPosition(latitude, longitude, height);
19479 final var nedVelocity = new NEDVelocity();
19480 final var ecefPosition = new ECEFPosition();
19481 final var ecefVelocity = new ECEFVelocity();
19482 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19483 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19484 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19485 final var gravityNorm = gravity.getNormAsAcceleration();
19486
19487 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
19488 biasX, biasY, biasZ);
19489
19490
19491 assertEquals(biasX, calibrator.getBiasX(), 0.0);
19492 assertEquals(biasY, calibrator.getBiasY(), 0.0);
19493 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19494 final var bx1 = calibrator.getBiasXAsAcceleration();
19495 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19496 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19497 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19498 calibrator.getBiasXAsAcceleration(bx2);
19499 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19500 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19501 final var by1 = calibrator.getBiasYAsAcceleration();
19502 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19503 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19504 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19505 calibrator.getBiasYAsAcceleration(by2);
19506 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19507 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19508 final var bz1 = calibrator.getBiasZAsAcceleration();
19509 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19510 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19511 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19512 calibrator.getBiasZAsAcceleration(bz2);
19513 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19514 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19515 final var biasTriad1 = calibrator.getBiasAsTriad();
19516 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19517 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19518 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19519 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19520 final var biasTriad2 = new AccelerationTriad();
19521 calibrator.getBiasAsTriad(biasTriad2);
19522 assertEquals(biasTriad1, biasTriad2);
19523 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19524 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19525 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19526 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19527 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19528 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19529 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19530 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19531 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19532 final var bias1 = calibrator.getBias();
19533 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19534 final var bias2 = new double[3];
19535 calibrator.getBias(bias2);
19536 assertArrayEquals(bias1, bias2, 0.0);
19537 final var b1 = calibrator.getBiasAsMatrix();
19538 assertEquals(b1, ba);
19539 final var b2 = new Matrix(3, 1);
19540 calibrator.getBiasAsMatrix(b2);
19541 assertEquals(b1, b2);
19542 final var ma1 = calibrator.getInitialMa();
19543 assertEquals(new Matrix(3, 3), ma1);
19544 final var ma2 = new Matrix(3, 3);
19545 calibrator.getInitialMa(ma2);
19546 assertEquals(ma1, ma2);
19547 assertNull(calibrator.getMeasurements());
19548 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19549 calibrator.getMeasurementType());
19550 assertFalse(calibrator.isOrderedMeasurementsRequired());
19551 assertFalse(calibrator.isQualityScoresRequired());
19552 assertTrue(calibrator.isCommonAxisUsed());
19553 assertNull(calibrator.getListener());
19554 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
19555 assertFalse(calibrator.isReady());
19556 assertFalse(calibrator.isRunning());
19557 assertNull(calibrator.getEstimatedMa());
19558 assertNull(calibrator.getEstimatedSx());
19559 assertNull(calibrator.getEstimatedSy());
19560 assertNull(calibrator.getEstimatedSz());
19561 assertNull(calibrator.getEstimatedMxy());
19562 assertNull(calibrator.getEstimatedMxz());
19563 assertNull(calibrator.getEstimatedMyx());
19564 assertNull(calibrator.getEstimatedMyz());
19565 assertNull(calibrator.getEstimatedMzx());
19566 assertNull(calibrator.getEstimatedMzy());
19567 assertNull(calibrator.getEstimatedCovariance());
19568 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19569 assertNotNull(calibrator.getGroundTruthGravityNorm());
19570 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19571 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19572 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19573 ABSOLUTE_ERROR));
19574 final var g = new Acceleration(0.0, AccelerationUnit.G);
19575 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19576 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19577 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19578
19579
19580 final var invalidGravityNorm = new Acceleration(
19581 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19582
19583 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19584 invalidGravityNorm, true, biasX, biasY, biasZ));
19585 }
19586
19587 @Test
19588 void testConstructor170() throws WrongSizeException {
19589 final var ba = generateBa();
19590 final var biasX = ba.getElementAtIndex(0);
19591 final var biasY = ba.getElementAtIndex(1);
19592 final var biasZ = ba.getElementAtIndex(2);
19593
19594 final var randomizer = new UniformRandomizer();
19595 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19596 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19597 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19598 final var nedPosition = new NEDPosition(latitude, longitude, height);
19599 final var nedVelocity = new NEDVelocity();
19600 final var ecefPosition = new ECEFPosition();
19601 final var ecefVelocity = new ECEFVelocity();
19602 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19603 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19604 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19605 final var gravityNorm = gravity.getNormAsAcceleration();
19606
19607 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
19608 biasX, biasY, biasZ, this);
19609
19610
19611 assertEquals(biasX, calibrator.getBiasX(), 0.0);
19612 assertEquals(biasY, calibrator.getBiasY(), 0.0);
19613 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19614 final var bx1 = calibrator.getBiasXAsAcceleration();
19615 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19616 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19617 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19618 calibrator.getBiasXAsAcceleration(bx2);
19619 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19620 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19621 final var by1 = calibrator.getBiasYAsAcceleration();
19622 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19623 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19624 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19625 calibrator.getBiasYAsAcceleration(by2);
19626 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19627 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19628 final var bz1 = calibrator.getBiasZAsAcceleration();
19629 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19630 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19631 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19632 calibrator.getBiasZAsAcceleration(bz2);
19633 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19634 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19635 final var biasTriad1 = calibrator.getBiasAsTriad();
19636 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19637 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19638 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19639 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19640 final var biasTriad2 = new AccelerationTriad();
19641 calibrator.getBiasAsTriad(biasTriad2);
19642 assertEquals(biasTriad1, biasTriad2);
19643 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19644 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19645 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19646 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19647 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19648 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19649 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19650 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19651 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19652 final var bias1 = calibrator.getBias();
19653 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19654 final var bias2 = new double[3];
19655 calibrator.getBias(bias2);
19656 assertArrayEquals(bias1, bias2, 0.0);
19657 final var b1 = calibrator.getBiasAsMatrix();
19658 assertEquals(b1, ba);
19659 final var b2 = new Matrix(3, 1);
19660 calibrator.getBiasAsMatrix(b2);
19661 assertEquals(b1, b2);
19662 final var ma1 = calibrator.getInitialMa();
19663 assertEquals(new Matrix(3, 3), ma1);
19664 final var ma2 = new Matrix(3, 3);
19665 calibrator.getInitialMa(ma2);
19666 assertEquals(ma1, ma2);
19667 assertNull(calibrator.getMeasurements());
19668 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19669 calibrator.getMeasurementType());
19670 assertFalse(calibrator.isOrderedMeasurementsRequired());
19671 assertFalse(calibrator.isQualityScoresRequired());
19672 assertTrue(calibrator.isCommonAxisUsed());
19673 assertSame(this, calibrator.getListener());
19674 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
19675 assertFalse(calibrator.isReady());
19676 assertFalse(calibrator.isRunning());
19677 assertNull(calibrator.getEstimatedMa());
19678 assertNull(calibrator.getEstimatedSx());
19679 assertNull(calibrator.getEstimatedSy());
19680 assertNull(calibrator.getEstimatedSz());
19681 assertNull(calibrator.getEstimatedMxy());
19682 assertNull(calibrator.getEstimatedMxz());
19683 assertNull(calibrator.getEstimatedMyx());
19684 assertNull(calibrator.getEstimatedMyz());
19685 assertNull(calibrator.getEstimatedMzx());
19686 assertNull(calibrator.getEstimatedMzy());
19687 assertNull(calibrator.getEstimatedCovariance());
19688 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19689 assertNotNull(calibrator.getGroundTruthGravityNorm());
19690 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19691 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19692 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19693 ABSOLUTE_ERROR));
19694 final var g = new Acceleration(0.0, AccelerationUnit.G);
19695 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19696 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19697 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19698
19699
19700 final var invalidGravityNorm = new Acceleration(
19701 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19702
19703 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19704 invalidGravityNorm, true, biasX, biasY, biasZ, this));
19705 }
19706
19707 @Test
19708 void testConstructor171() throws WrongSizeException {
19709 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
19710
19711 final var ba = generateBa();
19712 final var biasX = ba.getElementAtIndex(0);
19713 final var biasY = ba.getElementAtIndex(1);
19714 final var biasZ = ba.getElementAtIndex(2);
19715
19716 final var randomizer = new UniformRandomizer();
19717 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19718 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19719 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19720 final var nedPosition = new NEDPosition(latitude, longitude, height);
19721 final var nedVelocity = new NEDVelocity();
19722 final var ecefPosition = new ECEFPosition();
19723 final var ecefVelocity = new ECEFVelocity();
19724 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19725 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19726 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19727 final var gravityNorm = gravity.getNormAsAcceleration();
19728
19729 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
19730 true, biasX, biasY, biasZ);
19731
19732
19733 assertEquals(biasX, calibrator.getBiasX(), 0.0);
19734 assertEquals(biasY, calibrator.getBiasY(), 0.0);
19735 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19736 final var bx1 = calibrator.getBiasXAsAcceleration();
19737 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19738 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19739 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19740 calibrator.getBiasXAsAcceleration(bx2);
19741 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19742 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19743 final var by1 = calibrator.getBiasYAsAcceleration();
19744 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19745 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19746 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19747 calibrator.getBiasYAsAcceleration(by2);
19748 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19749 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19750 final var bz1 = calibrator.getBiasZAsAcceleration();
19751 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19752 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19753 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19754 calibrator.getBiasZAsAcceleration(bz2);
19755 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19756 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19757 final var biasTriad1 = calibrator.getBiasAsTriad();
19758 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19759 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19760 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19761 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19762 final var biasTriad2 = new AccelerationTriad();
19763 calibrator.getBiasAsTriad(biasTriad2);
19764 assertEquals(biasTriad1, biasTriad2);
19765 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19766 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19767 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19768 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19769 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19770 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19771 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19772 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19773 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19774 final var bias1 = calibrator.getBias();
19775 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19776 final var bias2 = new double[3];
19777 calibrator.getBias(bias2);
19778 assertArrayEquals(bias1, bias2, 0.0);
19779 final var b1 = calibrator.getBiasAsMatrix();
19780 assertEquals(b1, ba);
19781 final var b2 = new Matrix(3, 1);
19782 calibrator.getBiasAsMatrix(b2);
19783 assertEquals(b1, b2);
19784 final var ma1 = calibrator.getInitialMa();
19785 assertEquals(new Matrix(3, 3), ma1);
19786 final var ma2 = new Matrix(3, 3);
19787 calibrator.getInitialMa(ma2);
19788 assertEquals(ma1, ma2);
19789 assertSame(measurements, calibrator.getMeasurements());
19790 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19791 calibrator.getMeasurementType());
19792 assertFalse(calibrator.isOrderedMeasurementsRequired());
19793 assertFalse(calibrator.isQualityScoresRequired());
19794 assertTrue(calibrator.isCommonAxisUsed());
19795 assertNull(calibrator.getListener());
19796 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
19797 assertFalse(calibrator.isReady());
19798 assertFalse(calibrator.isRunning());
19799 assertNull(calibrator.getEstimatedMa());
19800 assertNull(calibrator.getEstimatedSx());
19801 assertNull(calibrator.getEstimatedSy());
19802 assertNull(calibrator.getEstimatedSz());
19803 assertNull(calibrator.getEstimatedMxy());
19804 assertNull(calibrator.getEstimatedMxz());
19805 assertNull(calibrator.getEstimatedMyx());
19806 assertNull(calibrator.getEstimatedMyz());
19807 assertNull(calibrator.getEstimatedMzx());
19808 assertNull(calibrator.getEstimatedMzy());
19809 assertNull(calibrator.getEstimatedCovariance());
19810 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19811 assertNotNull(calibrator.getGroundTruthGravityNorm());
19812 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19813 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19814 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19815 ABSOLUTE_ERROR));
19816 final var g = new Acceleration(0.0, AccelerationUnit.G);
19817 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19818 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19819 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19820
19821
19822 final var invalidGravityNorm = new Acceleration(
19823 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19824
19825 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19826 invalidGravityNorm, measurements, true, biasX, biasY, biasZ));
19827 }
19828
19829 @Test
19830 void testConstructor172() throws WrongSizeException {
19831 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
19832
19833 final var ba = generateBa();
19834 final var biasX = ba.getElementAtIndex(0);
19835 final var biasY = ba.getElementAtIndex(1);
19836 final var biasZ = ba.getElementAtIndex(2);
19837
19838 final var randomizer = new UniformRandomizer();
19839 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19840 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19841 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19842 final var nedPosition = new NEDPosition(latitude, longitude, height);
19843 final var nedVelocity = new NEDVelocity();
19844 final var ecefPosition = new ECEFPosition();
19845 final var ecefVelocity = new ECEFVelocity();
19846 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19847 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19848 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19849 final var gravityNorm = gravity.getNormAsAcceleration();
19850
19851 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, true,
19852 biasX, biasY, biasZ, this);
19853
19854
19855 assertEquals(biasX, calibrator.getBiasX(), 0.0);
19856 assertEquals(biasY, calibrator.getBiasY(), 0.0);
19857 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19858 final var bx1 = calibrator.getBiasXAsAcceleration();
19859 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19860 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19861 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19862 calibrator.getBiasXAsAcceleration(bx2);
19863 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19864 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19865 final var by1 = calibrator.getBiasYAsAcceleration();
19866 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19867 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19868 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19869 calibrator.getBiasYAsAcceleration(by2);
19870 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19871 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19872 final var bz1 = calibrator.getBiasZAsAcceleration();
19873 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19874 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19875 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19876 calibrator.getBiasZAsAcceleration(bz2);
19877 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19878 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19879 final var biasTriad1 = calibrator.getBiasAsTriad();
19880 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19881 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19882 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19883 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19884 final var biasTriad2 = new AccelerationTriad();
19885 calibrator.getBiasAsTriad(biasTriad2);
19886 assertEquals(biasTriad1, biasTriad2);
19887 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19888 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19889 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19890 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19891 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19892 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19893 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19894 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19895 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19896 final var bias1 = calibrator.getBias();
19897 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19898 final var bias2 = new double[3];
19899 calibrator.getBias(bias2);
19900 assertArrayEquals(bias1, bias2, 0.0);
19901 final var b1 = calibrator.getBiasAsMatrix();
19902 assertEquals(b1, ba);
19903 final var b2 = new Matrix(3, 1);
19904 calibrator.getBiasAsMatrix(b2);
19905 assertEquals(b1, b2);
19906 final var ma1 = calibrator.getInitialMa();
19907 assertEquals(new Matrix(3, 3), ma1);
19908 final var ma2 = new Matrix(3, 3);
19909 calibrator.getInitialMa(ma2);
19910 assertEquals(ma1, ma2);
19911 assertSame(measurements, calibrator.getMeasurements());
19912 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19913 calibrator.getMeasurementType());
19914 assertFalse(calibrator.isOrderedMeasurementsRequired());
19915 assertFalse(calibrator.isQualityScoresRequired());
19916 assertTrue(calibrator.isCommonAxisUsed());
19917 assertSame(this, calibrator.getListener());
19918 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
19919 assertFalse(calibrator.isReady());
19920 assertFalse(calibrator.isRunning());
19921 assertNull(calibrator.getEstimatedMa());
19922 assertNull(calibrator.getEstimatedSx());
19923 assertNull(calibrator.getEstimatedSy());
19924 assertNull(calibrator.getEstimatedSz());
19925 assertNull(calibrator.getEstimatedMxy());
19926 assertNull(calibrator.getEstimatedMxz());
19927 assertNull(calibrator.getEstimatedMyx());
19928 assertNull(calibrator.getEstimatedMyz());
19929 assertNull(calibrator.getEstimatedMzx());
19930 assertNull(calibrator.getEstimatedMzy());
19931 assertNull(calibrator.getEstimatedCovariance());
19932 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19933 assertNotNull(calibrator.getGroundTruthGravityNorm());
19934 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19935 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19936 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19937 ABSOLUTE_ERROR));
19938 final var g = new Acceleration(0.0, AccelerationUnit.G);
19939 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19940 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19941 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19942
19943
19944 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19945
19946 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19947 invalidGravityNorm, measurements, true, biasX, biasY, biasZ, this));
19948 }
19949
19950 @Test
19951 void testConstructor173() throws WrongSizeException {
19952 final var ba = generateBa();
19953 final var biasX = ba.getElementAtIndex(0);
19954 final var biasY = ba.getElementAtIndex(1);
19955 final var biasZ = ba.getElementAtIndex(2);
19956
19957 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
19958 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
19959 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
19960
19961 final var randomizer = new UniformRandomizer();
19962 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19963 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19964 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19965 final var nedPosition = new NEDPosition(latitude, longitude, height);
19966 final var nedVelocity = new NEDVelocity();
19967 final var ecefPosition = new ECEFPosition();
19968 final var ecefVelocity = new ECEFVelocity();
19969 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19970 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19971 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19972 final var gravityNorm = gravity.getNormAsAcceleration();
19973
19974 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz);
19975
19976
19977 assertEquals(biasX, calibrator.getBiasX(), 0.0);
19978 assertEquals(biasY, calibrator.getBiasY(), 0.0);
19979 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19980 final var bx1 = calibrator.getBiasXAsAcceleration();
19981 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19982 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19983 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19984 calibrator.getBiasXAsAcceleration(bx2);
19985 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19986 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19987 final var by1 = calibrator.getBiasYAsAcceleration();
19988 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19989 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19990 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19991 calibrator.getBiasYAsAcceleration(by2);
19992 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19993 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19994 final var bz1 = calibrator.getBiasZAsAcceleration();
19995 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19996 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19997 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19998 calibrator.getBiasZAsAcceleration(bz2);
19999 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20000 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20001 final var biasTriad1 = calibrator.getBiasAsTriad();
20002 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20003 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20004 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20005 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20006 final var biasTriad2 = new AccelerationTriad();
20007 calibrator.getBiasAsTriad(biasTriad2);
20008 assertEquals(biasTriad1, biasTriad2);
20009 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20010 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20011 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20012 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20013 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20014 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20015 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20016 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20017 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20018 final var bias1 = calibrator.getBias();
20019 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20020 final var bias2 = new double[3];
20021 calibrator.getBias(bias2);
20022 assertArrayEquals(bias1, bias2, 0.0);
20023 final var b1 = calibrator.getBiasAsMatrix();
20024 assertEquals(b1, ba);
20025 final var b2 = new Matrix(3, 1);
20026 calibrator.getBiasAsMatrix(b2);
20027 assertEquals(b1, b2);
20028 final var ma1 = calibrator.getInitialMa();
20029 assertEquals(new Matrix(3, 3), ma1);
20030 final var ma2 = new Matrix(3, 3);
20031 calibrator.getInitialMa(ma2);
20032 assertEquals(ma1, ma2);
20033 assertNull(calibrator.getMeasurements());
20034 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20035 calibrator.getMeasurementType());
20036 assertFalse(calibrator.isOrderedMeasurementsRequired());
20037 assertFalse(calibrator.isQualityScoresRequired());
20038 assertFalse(calibrator.isCommonAxisUsed());
20039 assertNull(calibrator.getListener());
20040 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
20041 assertFalse(calibrator.isReady());
20042 assertFalse(calibrator.isRunning());
20043 assertNull(calibrator.getEstimatedMa());
20044 assertNull(calibrator.getEstimatedSx());
20045 assertNull(calibrator.getEstimatedSy());
20046 assertNull(calibrator.getEstimatedSz());
20047 assertNull(calibrator.getEstimatedMxy());
20048 assertNull(calibrator.getEstimatedMxz());
20049 assertNull(calibrator.getEstimatedMyx());
20050 assertNull(calibrator.getEstimatedMyz());
20051 assertNull(calibrator.getEstimatedMzx());
20052 assertNull(calibrator.getEstimatedMzy());
20053 assertNull(calibrator.getEstimatedCovariance());
20054 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20055 assertNotNull(calibrator.getGroundTruthGravityNorm());
20056 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20057 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20058 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20059 ABSOLUTE_ERROR));
20060 final var g = new Acceleration(0.0, AccelerationUnit.G);
20061 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20062 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20063 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20064
20065
20066 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20067
20068 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20069 invalidGravityNorm, bx, by, bz));
20070 }
20071
20072 @Test
20073 void testConstructor174() throws WrongSizeException {
20074 final var ba = generateBa();
20075 final var biasX = ba.getElementAtIndex(0);
20076 final var biasY = ba.getElementAtIndex(1);
20077 final var biasZ = ba.getElementAtIndex(2);
20078
20079 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20080 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20081 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20082
20083 final var randomizer = new UniformRandomizer();
20084 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20085 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20086 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20087 final var nedPosition = new NEDPosition(latitude, longitude, height);
20088 final var nedVelocity = new NEDVelocity();
20089 final var ecefPosition = new ECEFPosition();
20090 final var ecefVelocity = new ECEFVelocity();
20091 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20092 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20093 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20094 final var gravityNorm = gravity.getNormAsAcceleration();
20095
20096 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz,
20097 this);
20098
20099
20100 assertEquals(biasX, calibrator.getBiasX(), 0.0);
20101 assertEquals(biasY, calibrator.getBiasY(), 0.0);
20102 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20103 final var bx1 = calibrator.getBiasXAsAcceleration();
20104 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20105 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20106 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20107 calibrator.getBiasXAsAcceleration(bx2);
20108 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20109 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20110 final var by1 = calibrator.getBiasYAsAcceleration();
20111 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20112 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20113 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20114 calibrator.getBiasYAsAcceleration(by2);
20115 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20116 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20117 final var bz1 = calibrator.getBiasZAsAcceleration();
20118 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20119 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20120 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20121 calibrator.getBiasZAsAcceleration(bz2);
20122 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20123 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20124 final var biasTriad1 = calibrator.getBiasAsTriad();
20125 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20126 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20127 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20128 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20129 final var biasTriad2 = new AccelerationTriad();
20130 calibrator.getBiasAsTriad(biasTriad2);
20131 assertEquals(biasTriad1, biasTriad2);
20132 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20133 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20134 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20135 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20136 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20137 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20138 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20139 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20140 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20141 final var bias1 = calibrator.getBias();
20142 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20143 final var bias2 = new double[3];
20144 calibrator.getBias(bias2);
20145 assertArrayEquals(bias1, bias2, 0.0);
20146 final var b1 = calibrator.getBiasAsMatrix();
20147 assertEquals(b1, ba);
20148 final var b2 = new Matrix(3, 1);
20149 calibrator.getBiasAsMatrix(b2);
20150 assertEquals(b1, b2);
20151 final var ma1 = calibrator.getInitialMa();
20152 assertEquals(new Matrix(3, 3), ma1);
20153 final var ma2 = new Matrix(3, 3);
20154 calibrator.getInitialMa(ma2);
20155 assertEquals(ma1, ma2);
20156 assertNull(calibrator.getMeasurements());
20157 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20158 calibrator.getMeasurementType());
20159 assertFalse(calibrator.isOrderedMeasurementsRequired());
20160 assertFalse(calibrator.isQualityScoresRequired());
20161 assertFalse(calibrator.isCommonAxisUsed());
20162 assertSame(this, calibrator.getListener());
20163 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
20164 assertFalse(calibrator.isReady());
20165 assertFalse(calibrator.isRunning());
20166 assertNull(calibrator.getEstimatedMa());
20167 assertNull(calibrator.getEstimatedSx());
20168 assertNull(calibrator.getEstimatedSy());
20169 assertNull(calibrator.getEstimatedSz());
20170 assertNull(calibrator.getEstimatedMxy());
20171 assertNull(calibrator.getEstimatedMxz());
20172 assertNull(calibrator.getEstimatedMyx());
20173 assertNull(calibrator.getEstimatedMyz());
20174 assertNull(calibrator.getEstimatedMzx());
20175 assertNull(calibrator.getEstimatedMzy());
20176 assertNull(calibrator.getEstimatedCovariance());
20177 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20178 assertNotNull(calibrator.getGroundTruthGravityNorm());
20179 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20180 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20181 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20182 ABSOLUTE_ERROR));
20183 final var g = new Acceleration(0.0, AccelerationUnit.G);
20184 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20185 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20186 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20187
20188
20189 final var invalidGravityNorm = new Acceleration(
20190 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20191
20192 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20193 invalidGravityNorm, bx, by, bz, this));
20194 }
20195
20196 @Test
20197 void testConstructor175() throws WrongSizeException {
20198 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
20199
20200 final var ba = generateBa();
20201 final var biasX = ba.getElementAtIndex(0);
20202 final var biasY = ba.getElementAtIndex(1);
20203 final var biasZ = ba.getElementAtIndex(2);
20204
20205 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20206 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20207 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20208
20209 final var randomizer = new UniformRandomizer();
20210 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20211 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20212 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20213 final var nedPosition = new NEDPosition(latitude, longitude, height);
20214 final var nedVelocity = new NEDVelocity();
20215 final var ecefPosition = new ECEFPosition();
20216 final var ecefVelocity = new ECEFVelocity();
20217 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20218 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20219 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20220 final var gravityNorm = gravity.getNormAsAcceleration();
20221
20222 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
20223 bx, by, bz);
20224
20225
20226 assertEquals(biasX, calibrator.getBiasX(), 0.0);
20227 assertEquals(biasY, calibrator.getBiasY(), 0.0);
20228 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20229 final var bx1 = calibrator.getBiasXAsAcceleration();
20230 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20231 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20232 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20233 calibrator.getBiasXAsAcceleration(bx2);
20234 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20235 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20236 final var by1 = calibrator.getBiasYAsAcceleration();
20237 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20238 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20239 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20240 calibrator.getBiasYAsAcceleration(by2);
20241 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20242 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20243 final var bz1 = calibrator.getBiasZAsAcceleration();
20244 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20245 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20246 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20247 calibrator.getBiasZAsAcceleration(bz2);
20248 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20249 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20250 final var biasTriad1 = calibrator.getBiasAsTriad();
20251 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20252 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20253 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20254 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20255 final var biasTriad2 = new AccelerationTriad();
20256 calibrator.getBiasAsTriad(biasTriad2);
20257 assertEquals(biasTriad1, biasTriad2);
20258 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20259 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20260 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20261 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20262 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20263 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20264 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20265 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20266 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20267 final var bias1 = calibrator.getBias();
20268 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20269 final var bias2 = new double[3];
20270 calibrator.getBias(bias2);
20271 assertArrayEquals(bias1, bias2, 0.0);
20272 final var b1 = calibrator.getBiasAsMatrix();
20273 assertEquals(b1, ba);
20274 final var b2 = new Matrix(3, 1);
20275 calibrator.getBiasAsMatrix(b2);
20276 assertEquals(b1, b2);
20277 final var ma1 = calibrator.getInitialMa();
20278 assertEquals(new Matrix(3, 3), ma1);
20279 final var ma2 = new Matrix(3, 3);
20280 calibrator.getInitialMa(ma2);
20281 assertEquals(ma1, ma2);
20282 assertSame(measurements, calibrator.getMeasurements());
20283 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20284 calibrator.getMeasurementType());
20285 assertFalse(calibrator.isOrderedMeasurementsRequired());
20286 assertFalse(calibrator.isQualityScoresRequired());
20287 assertFalse(calibrator.isCommonAxisUsed());
20288 assertNull(calibrator.getListener());
20289 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
20290 assertFalse(calibrator.isReady());
20291 assertFalse(calibrator.isRunning());
20292 assertNull(calibrator.getEstimatedMa());
20293 assertNull(calibrator.getEstimatedSx());
20294 assertNull(calibrator.getEstimatedSy());
20295 assertNull(calibrator.getEstimatedSz());
20296 assertNull(calibrator.getEstimatedMxy());
20297 assertNull(calibrator.getEstimatedMxz());
20298 assertNull(calibrator.getEstimatedMyx());
20299 assertNull(calibrator.getEstimatedMyz());
20300 assertNull(calibrator.getEstimatedMzx());
20301 assertNull(calibrator.getEstimatedMzy());
20302 assertNull(calibrator.getEstimatedCovariance());
20303 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20304 assertNotNull(calibrator.getGroundTruthGravityNorm());
20305 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20306 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20307 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20308 ABSOLUTE_ERROR));
20309 final var g = new Acceleration(0.0, AccelerationUnit.G);
20310 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20311 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20312 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20313
20314
20315 final var invalidGravityNorm = new Acceleration(
20316 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20317
20318 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20319 invalidGravityNorm, measurements, bx, by, bz));
20320 }
20321
20322 @Test
20323 void testConstructor176() throws WrongSizeException {
20324 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
20325
20326 final var ba = generateBa();
20327 final var biasX = ba.getElementAtIndex(0);
20328 final var biasY = ba.getElementAtIndex(1);
20329 final var biasZ = ba.getElementAtIndex(2);
20330
20331 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20332 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20333 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20334
20335 final var randomizer = new UniformRandomizer();
20336 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20337 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20338 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20339 final var nedPosition = new NEDPosition(latitude, longitude, height);
20340 final var nedVelocity = new NEDVelocity();
20341 final var ecefPosition = new ECEFPosition();
20342 final var ecefVelocity = new ECEFVelocity();
20343 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20344 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20345 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20346 final var gravityNorm = gravity.getNormAsAcceleration();
20347
20348 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
20349 this);
20350
20351
20352 assertEquals(biasX, calibrator.getBiasX(), 0.0);
20353 assertEquals(biasY, calibrator.getBiasY(), 0.0);
20354 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20355 final var bx1 = calibrator.getBiasXAsAcceleration();
20356 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20357 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20358 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20359 calibrator.getBiasXAsAcceleration(bx2);
20360 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20361 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20362 final var by1 = calibrator.getBiasYAsAcceleration();
20363 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20364 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20365 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20366 calibrator.getBiasYAsAcceleration(by2);
20367 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20368 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20369 final var bz1 = calibrator.getBiasZAsAcceleration();
20370 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20371 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20372 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20373 calibrator.getBiasZAsAcceleration(bz2);
20374 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20375 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20376 final var biasTriad1 = calibrator.getBiasAsTriad();
20377 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20378 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20379 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20380 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20381 final var biasTriad2 = new AccelerationTriad();
20382 calibrator.getBiasAsTriad(biasTriad2);
20383 assertEquals(biasTriad1, biasTriad2);
20384 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20385 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20386 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20387 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20388 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20389 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20390 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20391 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20392 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20393 final var bias1 = calibrator.getBias();
20394 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20395 final var bias2 = new double[3];
20396 calibrator.getBias(bias2);
20397 assertArrayEquals(bias1, bias2, 0.0);
20398 final var b1 = calibrator.getBiasAsMatrix();
20399 assertEquals(b1, ba);
20400 final var b2 = new Matrix(3, 1);
20401 calibrator.getBiasAsMatrix(b2);
20402 assertEquals(b1, b2);
20403 final var ma1 = calibrator.getInitialMa();
20404 assertEquals(new Matrix(3, 3), ma1);
20405 final var ma2 = new Matrix(3, 3);
20406 calibrator.getInitialMa(ma2);
20407 assertEquals(ma1, ma2);
20408 assertSame(measurements, calibrator.getMeasurements());
20409 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20410 calibrator.getMeasurementType());
20411 assertFalse(calibrator.isOrderedMeasurementsRequired());
20412 assertFalse(calibrator.isQualityScoresRequired());
20413 assertFalse(calibrator.isCommonAxisUsed());
20414 assertSame(this, calibrator.getListener());
20415 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
20416 assertFalse(calibrator.isReady());
20417 assertFalse(calibrator.isRunning());
20418 assertNull(calibrator.getEstimatedMa());
20419 assertNull(calibrator.getEstimatedSx());
20420 assertNull(calibrator.getEstimatedSy());
20421 assertNull(calibrator.getEstimatedSz());
20422 assertNull(calibrator.getEstimatedMxy());
20423 assertNull(calibrator.getEstimatedMxz());
20424 assertNull(calibrator.getEstimatedMyx());
20425 assertNull(calibrator.getEstimatedMyz());
20426 assertNull(calibrator.getEstimatedMzx());
20427 assertNull(calibrator.getEstimatedMzy());
20428 assertNull(calibrator.getEstimatedCovariance());
20429 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20430 assertNotNull(calibrator.getGroundTruthGravityNorm());
20431 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20432 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20433 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20434 ABSOLUTE_ERROR));
20435 final var g = new Acceleration(0.0, AccelerationUnit.G);
20436 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20437 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20438 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20439
20440
20441 final var invalidGravityNorm = new Acceleration(
20442 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20443
20444 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20445 invalidGravityNorm, measurements, bx, by, bz, this));
20446 }
20447
20448 @Test
20449 void testConstructor177() throws WrongSizeException {
20450 final var ba = generateBa();
20451 final var biasX = ba.getElementAtIndex(0);
20452 final var biasY = ba.getElementAtIndex(1);
20453 final var biasZ = ba.getElementAtIndex(2);
20454
20455 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20456 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20457 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20458
20459 final var randomizer = new UniformRandomizer();
20460 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20461 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20462 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20463 final var nedPosition = new NEDPosition(latitude, longitude, height);
20464 final var nedVelocity = new NEDVelocity();
20465 final var ecefPosition = new ECEFPosition();
20466 final var ecefVelocity = new ECEFVelocity();
20467 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20468 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20469 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20470 final var gravityNorm = gravity.getNormAsAcceleration();
20471
20472 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
20473 bx, by, bz);
20474
20475
20476 assertEquals(biasX, calibrator.getBiasX(), 0.0);
20477 assertEquals(biasY, calibrator.getBiasY(), 0.0);
20478 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20479 final var bx1 = calibrator.getBiasXAsAcceleration();
20480 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20481 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20482 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20483 calibrator.getBiasXAsAcceleration(bx2);
20484 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20485 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20486 final var by1 = calibrator.getBiasYAsAcceleration();
20487 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20488 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20489 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20490 calibrator.getBiasYAsAcceleration(by2);
20491 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20492 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20493 final var bz1 = calibrator.getBiasZAsAcceleration();
20494 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20495 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20496 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20497 calibrator.getBiasZAsAcceleration(bz2);
20498 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20499 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20500 final var biasTriad1 = calibrator.getBiasAsTriad();
20501 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20502 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20503 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20504 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20505 final var biasTriad2 = new AccelerationTriad();
20506 calibrator.getBiasAsTriad(biasTriad2);
20507 assertEquals(biasTriad1, biasTriad2);
20508 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20509 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20510 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20511 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20512 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20513 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20514 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20515 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20516 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20517 final var bias1 = calibrator.getBias();
20518 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20519 final var bias2 = new double[3];
20520 calibrator.getBias(bias2);
20521 assertArrayEquals(bias1, bias2, 0.0);
20522 final var b1 = calibrator.getBiasAsMatrix();
20523 assertEquals(b1, ba);
20524 final var b2 = new Matrix(3, 1);
20525 calibrator.getBiasAsMatrix(b2);
20526 assertEquals(b1, b2);
20527 final var ma1 = calibrator.getInitialMa();
20528 assertEquals(new Matrix(3, 3), ma1);
20529 final var ma2 = new Matrix(3, 3);
20530 calibrator.getInitialMa(ma2);
20531 assertEquals(ma1, ma2);
20532 assertNull(calibrator.getMeasurements());
20533 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20534 calibrator.getMeasurementType());
20535 assertFalse(calibrator.isOrderedMeasurementsRequired());
20536 assertFalse(calibrator.isQualityScoresRequired());
20537 assertTrue(calibrator.isCommonAxisUsed());
20538 assertNull(calibrator.getListener());
20539 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
20540 assertFalse(calibrator.isReady());
20541 assertFalse(calibrator.isRunning());
20542 assertNull(calibrator.getEstimatedMa());
20543 assertNull(calibrator.getEstimatedSx());
20544 assertNull(calibrator.getEstimatedSy());
20545 assertNull(calibrator.getEstimatedSz());
20546 assertNull(calibrator.getEstimatedMxy());
20547 assertNull(calibrator.getEstimatedMxz());
20548 assertNull(calibrator.getEstimatedMyx());
20549 assertNull(calibrator.getEstimatedMyz());
20550 assertNull(calibrator.getEstimatedMzx());
20551 assertNull(calibrator.getEstimatedMzy());
20552 assertNull(calibrator.getEstimatedCovariance());
20553 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20554 assertNotNull(calibrator.getGroundTruthGravityNorm());
20555 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20556 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20557 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20558 ABSOLUTE_ERROR));
20559 final var g = new Acceleration(0.0, AccelerationUnit.G);
20560 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20561 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20562 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20563
20564
20565 final var invalidGravityNorm = new Acceleration(
20566 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20567
20568 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20569 invalidGravityNorm, true, bx, by, bz));
20570 }
20571
20572 @Test
20573 void testConstructor178() throws WrongSizeException {
20574 final var ba = generateBa();
20575 final var biasX = ba.getElementAtIndex(0);
20576 final var biasY = ba.getElementAtIndex(1);
20577 final var biasZ = ba.getElementAtIndex(2);
20578
20579 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20580 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20581 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20582
20583 final var randomizer = new UniformRandomizer();
20584 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20585 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20586 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20587 final var nedPosition = new NEDPosition(latitude, longitude, height);
20588 final var nedVelocity = new NEDVelocity();
20589 final var ecefPosition = new ECEFPosition();
20590 final var ecefVelocity = new ECEFVelocity();
20591 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20592 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20593 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20594 final var gravityNorm = gravity.getNormAsAcceleration();
20595
20596 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
20597 bx, by, bz, this);
20598
20599
20600 assertEquals(biasX, calibrator.getBiasX(), 0.0);
20601 assertEquals(biasY, calibrator.getBiasY(), 0.0);
20602 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20603 final var bx1 = calibrator.getBiasXAsAcceleration();
20604 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20605 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20606 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20607 calibrator.getBiasXAsAcceleration(bx2);
20608 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20609 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20610 final var by1 = calibrator.getBiasYAsAcceleration();
20611 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20612 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20613 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20614 calibrator.getBiasYAsAcceleration(by2);
20615 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20616 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20617 final var bz1 = calibrator.getBiasZAsAcceleration();
20618 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20619 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20620 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20621 calibrator.getBiasZAsAcceleration(bz2);
20622 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20623 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20624 final var biasTriad1 = calibrator.getBiasAsTriad();
20625 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20626 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20627 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20628 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20629 final var biasTriad2 = new AccelerationTriad();
20630 calibrator.getBiasAsTriad(biasTriad2);
20631 assertEquals(biasTriad1, biasTriad2);
20632 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20633 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20634 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20635 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20636 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20637 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20638 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20639 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20640 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20641 final var bias1 = calibrator.getBias();
20642 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20643 final var bias2 = new double[3];
20644 calibrator.getBias(bias2);
20645 assertArrayEquals(bias1, bias2, 0.0);
20646 final var b1 = calibrator.getBiasAsMatrix();
20647 assertEquals(b1, ba);
20648 final var b2 = new Matrix(3, 1);
20649 calibrator.getBiasAsMatrix(b2);
20650 assertEquals(b1, b2);
20651 final var ma1 = calibrator.getInitialMa();
20652 assertEquals(new Matrix(3, 3), ma1);
20653 final var ma2 = new Matrix(3, 3);
20654 calibrator.getInitialMa(ma2);
20655 assertEquals(ma1, ma2);
20656 assertNull(calibrator.getMeasurements());
20657 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20658 calibrator.getMeasurementType());
20659 assertFalse(calibrator.isOrderedMeasurementsRequired());
20660 assertFalse(calibrator.isQualityScoresRequired());
20661 assertTrue(calibrator.isCommonAxisUsed());
20662 assertSame(this, calibrator.getListener());
20663 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
20664 assertFalse(calibrator.isReady());
20665 assertFalse(calibrator.isRunning());
20666 assertNull(calibrator.getEstimatedMa());
20667 assertNull(calibrator.getEstimatedSx());
20668 assertNull(calibrator.getEstimatedSy());
20669 assertNull(calibrator.getEstimatedSz());
20670 assertNull(calibrator.getEstimatedMxy());
20671 assertNull(calibrator.getEstimatedMxz());
20672 assertNull(calibrator.getEstimatedMyx());
20673 assertNull(calibrator.getEstimatedMyz());
20674 assertNull(calibrator.getEstimatedMzx());
20675 assertNull(calibrator.getEstimatedMzy());
20676 assertNull(calibrator.getEstimatedCovariance());
20677 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20678 assertNotNull(calibrator.getGroundTruthGravityNorm());
20679 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20680 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20681 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20682 ABSOLUTE_ERROR));
20683 final var g = new Acceleration(0.0, AccelerationUnit.G);
20684 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20685 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20686 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20687
20688
20689 final var invalidGravityNorm = new Acceleration(
20690 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20691
20692 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20693 invalidGravityNorm, true, bx, by, bz, this));
20694 }
20695
20696 @Test
20697 void testConstructor179() throws WrongSizeException {
20698 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
20699
20700 final var ba = generateBa();
20701 final var biasX = ba.getElementAtIndex(0);
20702 final var biasY = ba.getElementAtIndex(1);
20703 final var biasZ = ba.getElementAtIndex(2);
20704
20705 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20706 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20707 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20708
20709 final var randomizer = new UniformRandomizer();
20710 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20711 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20712 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20713 final var nedPosition = new NEDPosition(latitude, longitude, height);
20714 final var nedVelocity = new NEDVelocity();
20715 final var ecefPosition = new ECEFPosition();
20716 final var ecefVelocity = new ECEFVelocity();
20717 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20718 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20719 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20720 final var gravityNorm = gravity.getNormAsAcceleration();
20721
20722 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
20723 true, bx, by, bz);
20724
20725
20726 assertEquals(biasX, calibrator.getBiasX(), 0.0);
20727 assertEquals(biasY, calibrator.getBiasY(), 0.0);
20728 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20729 final var bx1 = calibrator.getBiasXAsAcceleration();
20730 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20731 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20732 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20733 calibrator.getBiasXAsAcceleration(bx2);
20734 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20735 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20736 final var by1 = calibrator.getBiasYAsAcceleration();
20737 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20738 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20739 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20740 calibrator.getBiasYAsAcceleration(by2);
20741 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20742 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20743 final var bz1 = calibrator.getBiasZAsAcceleration();
20744 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20745 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20746 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20747 calibrator.getBiasZAsAcceleration(bz2);
20748 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20749 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20750 final var biasTriad1 = calibrator.getBiasAsTriad();
20751 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20752 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20753 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20754 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20755 final var biasTriad2 = new AccelerationTriad();
20756 calibrator.getBiasAsTriad(biasTriad2);
20757 assertEquals(biasTriad1, biasTriad2);
20758 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20759 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20760 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20761 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20762 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20763 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20764 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20765 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20766 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20767 final var bias1 = calibrator.getBias();
20768 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20769 final var bias2 = new double[3];
20770 calibrator.getBias(bias2);
20771 assertArrayEquals(bias1, bias2, 0.0);
20772 final var b1 = calibrator.getBiasAsMatrix();
20773 assertEquals(b1, ba);
20774 final var b2 = new Matrix(3, 1);
20775 calibrator.getBiasAsMatrix(b2);
20776 assertEquals(b1, b2);
20777 final var ma1 = calibrator.getInitialMa();
20778 assertEquals(new Matrix(3, 3), ma1);
20779 final var ma2 = new Matrix(3, 3);
20780 calibrator.getInitialMa(ma2);
20781 assertEquals(ma1, ma2);
20782 assertSame(measurements, calibrator.getMeasurements());
20783 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20784 calibrator.getMeasurementType());
20785 assertFalse(calibrator.isOrderedMeasurementsRequired());
20786 assertFalse(calibrator.isQualityScoresRequired());
20787 assertTrue(calibrator.isCommonAxisUsed());
20788 assertNull(calibrator.getListener());
20789 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
20790 assertFalse(calibrator.isReady());
20791 assertFalse(calibrator.isRunning());
20792 assertNull(calibrator.getEstimatedMa());
20793 assertNull(calibrator.getEstimatedSx());
20794 assertNull(calibrator.getEstimatedSy());
20795 assertNull(calibrator.getEstimatedSz());
20796 assertNull(calibrator.getEstimatedMxy());
20797 assertNull(calibrator.getEstimatedMxz());
20798 assertNull(calibrator.getEstimatedMyx());
20799 assertNull(calibrator.getEstimatedMyz());
20800 assertNull(calibrator.getEstimatedMzx());
20801 assertNull(calibrator.getEstimatedMzy());
20802 assertNull(calibrator.getEstimatedCovariance());
20803 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20804 assertNotNull(calibrator.getGroundTruthGravityNorm());
20805 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20806 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20807 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20808 ABSOLUTE_ERROR));
20809 final var g = new Acceleration(0.0, AccelerationUnit.G);
20810 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20811 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20812 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20813
20814
20815 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20816
20817 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20818 invalidGravityNorm, measurements, true, bx, by, bz));
20819 }
20820
20821 @Test
20822 void testConstructor180() throws WrongSizeException {
20823 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
20824
20825 final var ba = generateBa();
20826 final var biasX = ba.getElementAtIndex(0);
20827 final var biasY = ba.getElementAtIndex(1);
20828 final var biasZ = ba.getElementAtIndex(2);
20829
20830 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20831 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20832 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20833
20834 final var randomizer = new UniformRandomizer();
20835 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20836 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20837 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20838 final var nedPosition = new NEDPosition(latitude, longitude, height);
20839 final var nedVelocity = new NEDVelocity();
20840 final var ecefPosition = new ECEFPosition();
20841 final var ecefVelocity = new ECEFVelocity();
20842 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20843 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20844 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20845 final var gravityNorm = gravity.getNormAsAcceleration();
20846
20847 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
20848 true, bx, by, bz, this);
20849
20850
20851 assertEquals(biasX, calibrator.getBiasX(), 0.0);
20852 assertEquals(biasY, calibrator.getBiasY(), 0.0);
20853 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20854 final var bx1 = calibrator.getBiasXAsAcceleration();
20855 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20856 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20857 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20858 calibrator.getBiasXAsAcceleration(bx2);
20859 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20860 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20861 final var by1 = calibrator.getBiasYAsAcceleration();
20862 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20863 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20864 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20865 calibrator.getBiasYAsAcceleration(by2);
20866 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20867 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20868 final var bz1 = calibrator.getBiasZAsAcceleration();
20869 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20870 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20871 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20872 calibrator.getBiasZAsAcceleration(bz2);
20873 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20874 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20875 final var biasTriad1 = calibrator.getBiasAsTriad();
20876 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20877 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20878 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20879 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20880 final var biasTriad2 = new AccelerationTriad();
20881 calibrator.getBiasAsTriad(biasTriad2);
20882 assertEquals(biasTriad1, biasTriad2);
20883 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20884 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20885 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20886 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20887 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20888 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20889 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20890 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20891 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20892 final var bias1 = calibrator.getBias();
20893 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20894 final var bias2 = new double[3];
20895 calibrator.getBias(bias2);
20896 assertArrayEquals(bias1, bias2, 0.0);
20897 final var b1 = calibrator.getBiasAsMatrix();
20898 assertEquals(b1, ba);
20899 final var b2 = new Matrix(3, 1);
20900 calibrator.getBiasAsMatrix(b2);
20901 assertEquals(b1, b2);
20902 final var ma1 = calibrator.getInitialMa();
20903 assertEquals(new Matrix(3, 3), ma1);
20904 final var ma2 = new Matrix(3, 3);
20905 calibrator.getInitialMa(ma2);
20906 assertEquals(ma1, ma2);
20907 assertSame(measurements, calibrator.getMeasurements());
20908 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20909 calibrator.getMeasurementType());
20910 assertFalse(calibrator.isOrderedMeasurementsRequired());
20911 assertFalse(calibrator.isQualityScoresRequired());
20912 assertTrue(calibrator.isCommonAxisUsed());
20913 assertSame(this, calibrator.getListener());
20914 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
20915 assertFalse(calibrator.isReady());
20916 assertFalse(calibrator.isRunning());
20917 assertNull(calibrator.getEstimatedMa());
20918 assertNull(calibrator.getEstimatedSx());
20919 assertNull(calibrator.getEstimatedSy());
20920 assertNull(calibrator.getEstimatedSz());
20921 assertNull(calibrator.getEstimatedMxy());
20922 assertNull(calibrator.getEstimatedMxz());
20923 assertNull(calibrator.getEstimatedMyx());
20924 assertNull(calibrator.getEstimatedMyz());
20925 assertNull(calibrator.getEstimatedMzx());
20926 assertNull(calibrator.getEstimatedMzy());
20927 assertNull(calibrator.getEstimatedCovariance());
20928 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20929 assertNotNull(calibrator.getGroundTruthGravityNorm());
20930 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20931 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20932 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20933 ABSOLUTE_ERROR));
20934 final var g = new Acceleration(0.0, AccelerationUnit.G);
20935 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20936 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20937 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20938
20939
20940 final var invalidGravityNorm = new Acceleration(
20941 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20942
20943 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20944 invalidGravityNorm, measurements, true, bx, by, bz, this));
20945 }
20946
20947 @Test
20948 void testConstructor181() throws WrongSizeException {
20949 final var ba = generateBa();
20950 final var biasX = ba.getElementAtIndex(0);
20951 final var biasY = ba.getElementAtIndex(1);
20952 final var biasZ = ba.getElementAtIndex(2);
20953
20954 final var ma = generateMaCommonAxis();
20955 final var sx = ma.getElementAt(0, 0);
20956 final var sy = ma.getElementAt(1, 1);
20957 final var sz = ma.getElementAt(2, 2);
20958
20959 final var randomizer = new UniformRandomizer();
20960 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20961 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20962 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20963 final var nedPosition = new NEDPosition(latitude, longitude, height);
20964 final var nedVelocity = new NEDVelocity();
20965 final var ecefPosition = new ECEFPosition();
20966 final var ecefVelocity = new ECEFVelocity();
20967 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20968 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20969 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20970 final var gravityNorm = gravity.getNormAsAcceleration();
20971
20972 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ,
20973 sx, sy, sz);
20974
20975
20976 assertEquals(biasX, calibrator.getBiasX(), 0.0);
20977 assertEquals(biasY, calibrator.getBiasY(), 0.0);
20978 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20979 final var bx1 = calibrator.getBiasXAsAcceleration();
20980 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20981 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20982 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20983 calibrator.getBiasXAsAcceleration(bx2);
20984 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20985 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20986 final var by1 = calibrator.getBiasYAsAcceleration();
20987 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20988 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20989 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20990 calibrator.getBiasYAsAcceleration(by2);
20991 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20992 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20993 final var bz1 = calibrator.getBiasZAsAcceleration();
20994 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20995 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20996 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20997 calibrator.getBiasZAsAcceleration(bz2);
20998 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20999 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21000 final var biasTriad1 = calibrator.getBiasAsTriad();
21001 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21002 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21003 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21004 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21005 final var biasTriad2 = new AccelerationTriad();
21006 calibrator.getBiasAsTriad(biasTriad2);
21007 assertEquals(biasTriad1, biasTriad2);
21008 assertEquals(sx, calibrator.getInitialSx(), 0.0);
21009 assertEquals(sy, calibrator.getInitialSy(), 0.0);
21010 assertEquals(sz, calibrator.getInitialSz(), 0.0);
21011 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21012 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21013 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21014 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21015 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21016 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21017 final var bias1 = calibrator.getBias();
21018 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21019 final var bias2 = new double[3];
21020 calibrator.getBias(bias2);
21021 assertArrayEquals(bias1, bias2, 0.0);
21022 final var b1 = calibrator.getBiasAsMatrix();
21023 assertEquals(b1, ba);
21024 final var b2 = new Matrix(3, 1);
21025 calibrator.getBiasAsMatrix(b2);
21026 assertEquals(b1, b2);
21027 final var ma1 = calibrator.getInitialMa();
21028 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21029 final var ma2 = new Matrix(3, 3);
21030 calibrator.getInitialMa(ma2);
21031 assertEquals(ma1, ma2);
21032 assertNull(calibrator.getMeasurements());
21033 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21034 calibrator.getMeasurementType());
21035 assertFalse(calibrator.isOrderedMeasurementsRequired());
21036 assertFalse(calibrator.isQualityScoresRequired());
21037 assertFalse(calibrator.isCommonAxisUsed());
21038 assertNull(calibrator.getListener());
21039 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
21040 assertFalse(calibrator.isReady());
21041 assertFalse(calibrator.isRunning());
21042 assertNull(calibrator.getEstimatedMa());
21043 assertNull(calibrator.getEstimatedSx());
21044 assertNull(calibrator.getEstimatedSy());
21045 assertNull(calibrator.getEstimatedSz());
21046 assertNull(calibrator.getEstimatedMxy());
21047 assertNull(calibrator.getEstimatedMxz());
21048 assertNull(calibrator.getEstimatedMyx());
21049 assertNull(calibrator.getEstimatedMyz());
21050 assertNull(calibrator.getEstimatedMzx());
21051 assertNull(calibrator.getEstimatedMzy());
21052 assertNull(calibrator.getEstimatedCovariance());
21053 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21054 assertNotNull(calibrator.getGroundTruthGravityNorm());
21055 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21056 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21057 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21058 ABSOLUTE_ERROR));
21059 final var g = new Acceleration(0.0, AccelerationUnit.G);
21060 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21061 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21062 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21063
21064
21065 final var invalidGravityNorm = new Acceleration(
21066 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21067
21068 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21069 invalidGravityNorm, biasX, biasY, biasZ, sx, sy, sz));
21070 }
21071
21072 @Test
21073 void testConstructor182() throws WrongSizeException {
21074 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
21075
21076 final var ba = generateBa();
21077 final var biasX = ba.getElementAtIndex(0);
21078 final var biasY = ba.getElementAtIndex(1);
21079 final var biasZ = ba.getElementAtIndex(2);
21080
21081 final var ma = generateMaCommonAxis();
21082 final var sx = ma.getElementAt(0, 0);
21083 final var sy = ma.getElementAt(1, 1);
21084 final var sz = ma.getElementAt(2, 2);
21085
21086 final var randomizer = new UniformRandomizer();
21087 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21088 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21089 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21090 final var nedPosition = new NEDPosition(latitude, longitude, height);
21091 final var nedVelocity = new NEDVelocity();
21092 final var ecefPosition = new ECEFPosition();
21093 final var ecefVelocity = new ECEFVelocity();
21094 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21095 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21096 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21097 final var gravityNorm = gravity.getNormAsAcceleration();
21098
21099 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
21100 biasX, biasY, biasZ, sx, sy, sz);
21101
21102
21103 assertEquals(biasX, calibrator.getBiasX(), 0.0);
21104 assertEquals(biasY, calibrator.getBiasY(), 0.0);
21105 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21106 final var bx1 = calibrator.getBiasXAsAcceleration();
21107 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21108 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21109 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21110 calibrator.getBiasXAsAcceleration(bx2);
21111 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21112 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21113 final var by1 = calibrator.getBiasYAsAcceleration();
21114 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21115 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
21116 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21117 calibrator.getBiasYAsAcceleration(by2);
21118 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
21119 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
21120 final var bz1 = calibrator.getBiasZAsAcceleration();
21121 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
21122 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
21123 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21124 calibrator.getBiasZAsAcceleration(bz2);
21125 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
21126 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21127 final var biasTriad1 = calibrator.getBiasAsTriad();
21128 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21129 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21130 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21131 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21132 final var biasTriad2 = new AccelerationTriad();
21133 calibrator.getBiasAsTriad(biasTriad2);
21134 assertEquals(biasTriad1, biasTriad2);
21135 assertEquals(sx, calibrator.getInitialSx(), 0.0);
21136 assertEquals(sy, calibrator.getInitialSy(), 0.0);
21137 assertEquals(sz, calibrator.getInitialSz(), 0.0);
21138 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21139 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21140 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21141 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21142 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21143 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21144 final var bias1 = calibrator.getBias();
21145 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21146 final var bias2 = new double[3];
21147 calibrator.getBias(bias2);
21148 assertArrayEquals(bias1, bias2, 0.0);
21149 final var b1 = calibrator.getBiasAsMatrix();
21150 assertEquals(b1, ba);
21151 final var b2 = new Matrix(3, 1);
21152 calibrator.getBiasAsMatrix(b2);
21153 assertEquals(b1, b2);
21154 final var ma1 = calibrator.getInitialMa();
21155 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21156 final var ma2 = new Matrix(3, 3);
21157 calibrator.getInitialMa(ma2);
21158 assertEquals(ma1, ma2);
21159 assertSame(measurements, calibrator.getMeasurements());
21160 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21161 calibrator.getMeasurementType());
21162 assertFalse(calibrator.isOrderedMeasurementsRequired());
21163 assertFalse(calibrator.isQualityScoresRequired());
21164 assertFalse(calibrator.isCommonAxisUsed());
21165 assertNull(calibrator.getListener());
21166 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
21167 assertFalse(calibrator.isReady());
21168 assertFalse(calibrator.isRunning());
21169 assertNull(calibrator.getEstimatedMa());
21170 assertNull(calibrator.getEstimatedSx());
21171 assertNull(calibrator.getEstimatedSy());
21172 assertNull(calibrator.getEstimatedSz());
21173 assertNull(calibrator.getEstimatedMxy());
21174 assertNull(calibrator.getEstimatedMxz());
21175 assertNull(calibrator.getEstimatedMyx());
21176 assertNull(calibrator.getEstimatedMyz());
21177 assertNull(calibrator.getEstimatedMzx());
21178 assertNull(calibrator.getEstimatedMzy());
21179 assertNull(calibrator.getEstimatedCovariance());
21180 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21181 assertNotNull(calibrator.getGroundTruthGravityNorm());
21182 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21183 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21184 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21185 ABSOLUTE_ERROR));
21186 final var g = new Acceleration(0.0, AccelerationUnit.G);
21187 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21188 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21189 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21190
21191
21192 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21193
21194 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21195 invalidGravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz));
21196 }
21197
21198 @Test
21199 void testConstructor183() throws WrongSizeException {
21200 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
21201
21202 final var ba = generateBa();
21203 final var biasX = ba.getElementAtIndex(0);
21204 final var biasY = ba.getElementAtIndex(1);
21205 final var biasZ = ba.getElementAtIndex(2);
21206
21207 final var ma = generateMaCommonAxis();
21208 final var sx = ma.getElementAt(0, 0);
21209 final var sy = ma.getElementAt(1, 1);
21210 final var sz = ma.getElementAt(2, 2);
21211
21212 final var randomizer = new UniformRandomizer();
21213 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21214 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21215 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21216 final var nedPosition = new NEDPosition(latitude, longitude, height);
21217 final var nedVelocity = new NEDVelocity();
21218 final var ecefPosition = new ECEFPosition();
21219 final var ecefVelocity = new ECEFVelocity();
21220 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21221 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21222 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21223 final var gravityNorm = gravity.getNormAsAcceleration();
21224
21225 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
21226 biasX, biasY, biasZ, sx, sy, sz, this);
21227
21228
21229 assertEquals(biasX, calibrator.getBiasX(), 0.0);
21230 assertEquals(biasY, calibrator.getBiasY(), 0.0);
21231 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21232 final var bx1 = calibrator.getBiasXAsAcceleration();
21233 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21234 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21235 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21236 calibrator.getBiasXAsAcceleration(bx2);
21237 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21238 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21239 final var by1 = calibrator.getBiasYAsAcceleration();
21240 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21241 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
21242 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21243 calibrator.getBiasYAsAcceleration(by2);
21244 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
21245 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
21246 final var bz1 = calibrator.getBiasZAsAcceleration();
21247 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
21248 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
21249 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21250 calibrator.getBiasZAsAcceleration(bz2);
21251 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
21252 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21253 final var biasTriad1 = calibrator.getBiasAsTriad();
21254 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21255 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21256 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21257 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21258 final var biasTriad2 = new AccelerationTriad();
21259 calibrator.getBiasAsTriad(biasTriad2);
21260 assertEquals(biasTriad1, biasTriad2);
21261 assertEquals(sx, calibrator.getInitialSx(), 0.0);
21262 assertEquals(sy, calibrator.getInitialSy(), 0.0);
21263 assertEquals(sz, calibrator.getInitialSz(), 0.0);
21264 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21265 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21266 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21267 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21268 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21269 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21270 final var bias1 = calibrator.getBias();
21271 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21272 final var bias2 = new double[3];
21273 calibrator.getBias(bias2);
21274 assertArrayEquals(bias1, bias2, 0.0);
21275 final var b1 = calibrator.getBiasAsMatrix();
21276 assertEquals(b1, ba);
21277 final var b2 = new Matrix(3, 1);
21278 calibrator.getBiasAsMatrix(b2);
21279 assertEquals(b1, b2);
21280 final var ma1 = calibrator.getInitialMa();
21281 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21282 final var ma2 = new Matrix(3, 3);
21283 calibrator.getInitialMa(ma2);
21284 assertEquals(ma1, ma2);
21285 assertSame(measurements, calibrator.getMeasurements());
21286 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21287 calibrator.getMeasurementType());
21288 assertFalse(calibrator.isOrderedMeasurementsRequired());
21289 assertFalse(calibrator.isQualityScoresRequired());
21290 assertFalse(calibrator.isCommonAxisUsed());
21291 assertSame(this, calibrator.getListener());
21292 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
21293 assertFalse(calibrator.isReady());
21294 assertFalse(calibrator.isRunning());
21295 assertNull(calibrator.getEstimatedMa());
21296 assertNull(calibrator.getEstimatedSx());
21297 assertNull(calibrator.getEstimatedSy());
21298 assertNull(calibrator.getEstimatedSz());
21299 assertNull(calibrator.getEstimatedMxy());
21300 assertNull(calibrator.getEstimatedMxz());
21301 assertNull(calibrator.getEstimatedMyx());
21302 assertNull(calibrator.getEstimatedMyz());
21303 assertNull(calibrator.getEstimatedMzx());
21304 assertNull(calibrator.getEstimatedMzy());
21305 assertNull(calibrator.getEstimatedCovariance());
21306 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21307 assertNotNull(calibrator.getGroundTruthGravityNorm());
21308 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21309 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21310 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21311 ABSOLUTE_ERROR));
21312 final var g = new Acceleration(0.0, AccelerationUnit.G);
21313 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21314 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21315 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21316
21317
21318 final var invalidGravityNorm = new Acceleration(
21319 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21320
21321 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21322 invalidGravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz, this));
21323 }
21324
21325 @Test
21326 void testConstructor184() throws WrongSizeException {
21327 final var ba = generateBa();
21328 final var biasX = ba.getElementAtIndex(0);
21329 final var biasY = ba.getElementAtIndex(1);
21330 final var biasZ = ba.getElementAtIndex(2);
21331
21332 final var ma = generateMaCommonAxis();
21333 final var sx = ma.getElementAt(0, 0);
21334 final var sy = ma.getElementAt(1, 1);
21335 final var sz = ma.getElementAt(2, 2);
21336
21337 final var randomizer = new UniformRandomizer();
21338 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21339 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21340 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21341 final var nedPosition = new NEDPosition(latitude, longitude, height);
21342 final var nedVelocity = new NEDVelocity();
21343 final var ecefPosition = new ECEFPosition();
21344 final var ecefVelocity = new ECEFVelocity();
21345 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21346 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21347 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21348 final var gravityNorm = gravity.getNormAsAcceleration();
21349
21350 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
21351 biasX, biasY, biasZ, sx, sy, sz);
21352
21353
21354 assertEquals(biasX, calibrator.getBiasX(), 0.0);
21355 assertEquals(biasY, calibrator.getBiasY(), 0.0);
21356 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21357 final var bx1 = calibrator.getBiasXAsAcceleration();
21358 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21359 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21360 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21361 calibrator.getBiasXAsAcceleration(bx2);
21362 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21363 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21364 final var by1 = calibrator.getBiasYAsAcceleration();
21365 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21366 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
21367 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21368 calibrator.getBiasYAsAcceleration(by2);
21369 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
21370 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
21371 final var bz1 = calibrator.getBiasZAsAcceleration();
21372 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
21373 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
21374 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21375 calibrator.getBiasZAsAcceleration(bz2);
21376 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
21377 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21378 final var biasTriad1 = calibrator.getBiasAsTriad();
21379 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21380 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21381 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21382 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21383 final var biasTriad2 = new AccelerationTriad();
21384 calibrator.getBiasAsTriad(biasTriad2);
21385 assertEquals(biasTriad1, biasTriad2);
21386 assertEquals(sx, calibrator.getInitialSx(), 0.0);
21387 assertEquals(sy, calibrator.getInitialSy(), 0.0);
21388 assertEquals(sz, calibrator.getInitialSz(), 0.0);
21389 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21390 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21391 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21392 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21393 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21394 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21395 final var bias1 = calibrator.getBias();
21396 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21397 final var bias2 = new double[3];
21398 calibrator.getBias(bias2);
21399 assertArrayEquals(bias1, bias2, 0.0);
21400 final var b1 = calibrator.getBiasAsMatrix();
21401 assertEquals(b1, ba);
21402 final var b2 = new Matrix(3, 1);
21403 calibrator.getBiasAsMatrix(b2);
21404 assertEquals(b1, b2);
21405 final var ma1 = calibrator.getInitialMa();
21406 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21407 final var ma2 = new Matrix(3, 3);
21408 calibrator.getInitialMa(ma2);
21409 assertEquals(ma1, ma2);
21410 assertNull(calibrator.getMeasurements());
21411 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21412 calibrator.getMeasurementType());
21413 assertFalse(calibrator.isOrderedMeasurementsRequired());
21414 assertFalse(calibrator.isQualityScoresRequired());
21415 assertTrue(calibrator.isCommonAxisUsed());
21416 assertNull(calibrator.getListener());
21417 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
21418 assertFalse(calibrator.isReady());
21419 assertFalse(calibrator.isRunning());
21420 assertNull(calibrator.getEstimatedMa());
21421 assertNull(calibrator.getEstimatedSx());
21422 assertNull(calibrator.getEstimatedSy());
21423 assertNull(calibrator.getEstimatedSz());
21424 assertNull(calibrator.getEstimatedMxy());
21425 assertNull(calibrator.getEstimatedMxz());
21426 assertNull(calibrator.getEstimatedMyx());
21427 assertNull(calibrator.getEstimatedMyz());
21428 assertNull(calibrator.getEstimatedMzx());
21429 assertNull(calibrator.getEstimatedMzy());
21430 assertNull(calibrator.getEstimatedCovariance());
21431 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21432 assertNotNull(calibrator.getGroundTruthGravityNorm());
21433 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21434 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21435 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21436 ABSOLUTE_ERROR));
21437 final var g = new Acceleration(0.0, AccelerationUnit.G);
21438 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21439 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21440 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21441
21442
21443 final var invalidGravityNorm = new Acceleration(
21444 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21445
21446 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21447 invalidGravityNorm, true, biasX, biasY, biasZ, sx, sy, sz));
21448 }
21449
21450 @Test
21451 void testConstructor185() throws WrongSizeException {
21452 final var ba = generateBa();
21453 final var biasX = ba.getElementAtIndex(0);
21454 final var biasY = ba.getElementAtIndex(1);
21455 final var biasZ = ba.getElementAtIndex(2);
21456
21457 final var ma = generateMaCommonAxis();
21458 final var sx = ma.getElementAt(0, 0);
21459 final var sy = ma.getElementAt(1, 1);
21460 final var sz = ma.getElementAt(2, 2);
21461
21462 final var randomizer = new UniformRandomizer();
21463 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21464 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21465 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21466 final var nedPosition = new NEDPosition(latitude, longitude, height);
21467 final var nedVelocity = new NEDVelocity();
21468 final var ecefPosition = new ECEFPosition();
21469 final var ecefVelocity = new ECEFVelocity();
21470 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21471 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21472 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21473 final var gravityNorm = gravity.getNormAsAcceleration();
21474
21475 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
21476 biasX, biasY, biasZ, sx, sy, sz, this);
21477
21478
21479 assertEquals(biasX, calibrator.getBiasX(), 0.0);
21480 assertEquals(biasY, calibrator.getBiasY(), 0.0);
21481 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21482 final var bx1 = calibrator.getBiasXAsAcceleration();
21483 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21484 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21485 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21486 calibrator.getBiasXAsAcceleration(bx2);
21487 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21488 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21489 final var by1 = calibrator.getBiasYAsAcceleration();
21490 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21491 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
21492 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21493 calibrator.getBiasYAsAcceleration(by2);
21494 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
21495 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
21496 final var bz1 = calibrator.getBiasZAsAcceleration();
21497 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
21498 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
21499 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21500 calibrator.getBiasZAsAcceleration(bz2);
21501 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
21502 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21503 final var biasTriad1 = calibrator.getBiasAsTriad();
21504 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21505 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21506 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21507 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21508 final var biasTriad2 = new AccelerationTriad();
21509 calibrator.getBiasAsTriad(biasTriad2);
21510 assertEquals(biasTriad1, biasTriad2);
21511 assertEquals(sx, calibrator.getInitialSx(), 0.0);
21512 assertEquals(sy, calibrator.getInitialSy(), 0.0);
21513 assertEquals(sz, calibrator.getInitialSz(), 0.0);
21514 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21515 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21516 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21517 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21518 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21519 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21520 final var bias1 = calibrator.getBias();
21521 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21522 final var bias2 = new double[3];
21523 calibrator.getBias(bias2);
21524 assertArrayEquals(bias1, bias2, 0.0);
21525 final var b1 = calibrator.getBiasAsMatrix();
21526 assertEquals(b1, ba);
21527 final var b2 = new Matrix(3, 1);
21528 calibrator.getBiasAsMatrix(b2);
21529 assertEquals(b1, b2);
21530 final var ma1 = calibrator.getInitialMa();
21531 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21532 final var ma2 = new Matrix(3, 3);
21533 calibrator.getInitialMa(ma2);
21534 assertEquals(ma1, ma2);
21535 assertNull(calibrator.getMeasurements());
21536 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21537 calibrator.getMeasurementType());
21538 assertFalse(calibrator.isOrderedMeasurementsRequired());
21539 assertFalse(calibrator.isQualityScoresRequired());
21540 assertTrue(calibrator.isCommonAxisUsed());
21541 assertSame(this, calibrator.getListener());
21542 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
21543 assertFalse(calibrator.isReady());
21544 assertFalse(calibrator.isRunning());
21545 assertNull(calibrator.getEstimatedMa());
21546 assertNull(calibrator.getEstimatedSx());
21547 assertNull(calibrator.getEstimatedSy());
21548 assertNull(calibrator.getEstimatedSz());
21549 assertNull(calibrator.getEstimatedMxy());
21550 assertNull(calibrator.getEstimatedMxz());
21551 assertNull(calibrator.getEstimatedMyx());
21552 assertNull(calibrator.getEstimatedMyz());
21553 assertNull(calibrator.getEstimatedMzx());
21554 assertNull(calibrator.getEstimatedMzy());
21555 assertNull(calibrator.getEstimatedCovariance());
21556 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21557 assertNotNull(calibrator.getGroundTruthGravityNorm());
21558 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21559 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21560 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21561 ABSOLUTE_ERROR));
21562 final var g = new Acceleration(0.0, AccelerationUnit.G);
21563 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21564 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21565 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21566
21567
21568 final var invalidGravityNorm = new Acceleration(
21569 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21570
21571 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21572 invalidGravityNorm, true, biasX, biasY, biasZ, sx, sy, sz, this));
21573 }
21574
21575 @Test
21576 void testConstructor186() throws WrongSizeException {
21577 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
21578
21579 final var ba = generateBa();
21580 final var biasX = ba.getElementAtIndex(0);
21581 final var biasY = ba.getElementAtIndex(1);
21582 final var biasZ = ba.getElementAtIndex(2);
21583
21584 final var ma = generateMaCommonAxis();
21585 final var sx = ma.getElementAt(0, 0);
21586 final var sy = ma.getElementAt(1, 1);
21587 final var sz = ma.getElementAt(2, 2);
21588
21589 final var randomizer = new UniformRandomizer();
21590 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21591 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21592 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21593 final var nedPosition = new NEDPosition(latitude, longitude, height);
21594 final var nedVelocity = new NEDVelocity();
21595 final var ecefPosition = new ECEFPosition();
21596 final var ecefVelocity = new ECEFVelocity();
21597 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21598 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21599 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21600 final var gravityNorm = gravity.getNormAsAcceleration();
21601
21602 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
21603 true, biasX, biasY, biasZ, sx, sy, sz);
21604
21605
21606 assertEquals(biasX, calibrator.getBiasX(), 0.0);
21607 assertEquals(biasY, calibrator.getBiasY(), 0.0);
21608 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21609 final var bx1 = calibrator.getBiasXAsAcceleration();
21610 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21611 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21612 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21613 calibrator.getBiasXAsAcceleration(bx2);
21614 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21615 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21616 final var by1 = calibrator.getBiasYAsAcceleration();
21617 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21618 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
21619 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21620 calibrator.getBiasYAsAcceleration(by2);
21621 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
21622 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
21623 final var bz1 = calibrator.getBiasZAsAcceleration();
21624 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
21625 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
21626 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21627 calibrator.getBiasZAsAcceleration(bz2);
21628 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
21629 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21630 final var biasTriad1 = calibrator.getBiasAsTriad();
21631 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21632 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21633 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21634 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21635 final var biasTriad2 = new AccelerationTriad();
21636 calibrator.getBiasAsTriad(biasTriad2);
21637 assertEquals(biasTriad1, biasTriad2);
21638 assertEquals(sx, calibrator.getInitialSx(), 0.0);
21639 assertEquals(sy, calibrator.getInitialSy(), 0.0);
21640 assertEquals(sz, calibrator.getInitialSz(), 0.0);
21641 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21642 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21643 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21644 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21645 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21646 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21647 final var bias1 = calibrator.getBias();
21648 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21649 final var bias2 = new double[3];
21650 calibrator.getBias(bias2);
21651 assertArrayEquals(bias1, bias2, 0.0);
21652 final var b1 = calibrator.getBiasAsMatrix();
21653 assertEquals(b1, ba);
21654 final var b2 = new Matrix(3, 1);
21655 calibrator.getBiasAsMatrix(b2);
21656 assertEquals(b1, b2);
21657 final var ma1 = calibrator.getInitialMa();
21658 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21659 final var ma2 = new Matrix(3, 3);
21660 calibrator.getInitialMa(ma2);
21661 assertEquals(ma1, ma2);
21662 assertSame(measurements, calibrator.getMeasurements());
21663 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21664 calibrator.getMeasurementType());
21665 assertFalse(calibrator.isOrderedMeasurementsRequired());
21666 assertFalse(calibrator.isQualityScoresRequired());
21667 assertTrue(calibrator.isCommonAxisUsed());
21668 assertNull(calibrator.getListener());
21669 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
21670 assertFalse(calibrator.isReady());
21671 assertFalse(calibrator.isRunning());
21672 assertNull(calibrator.getEstimatedMa());
21673 assertNull(calibrator.getEstimatedSx());
21674 assertNull(calibrator.getEstimatedSy());
21675 assertNull(calibrator.getEstimatedSz());
21676 assertNull(calibrator.getEstimatedMxy());
21677 assertNull(calibrator.getEstimatedMxz());
21678 assertNull(calibrator.getEstimatedMyx());
21679 assertNull(calibrator.getEstimatedMyz());
21680 assertNull(calibrator.getEstimatedMzx());
21681 assertNull(calibrator.getEstimatedMzy());
21682 assertNull(calibrator.getEstimatedCovariance());
21683 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21684 assertNotNull(calibrator.getGroundTruthGravityNorm());
21685 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21686 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21687 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21688 ABSOLUTE_ERROR));
21689 final var g = new Acceleration(0.0, AccelerationUnit.G);
21690 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21691 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21692 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21693
21694
21695 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21696
21697 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21698 invalidGravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz));
21699 }
21700
21701 @Test
21702 void testConstructor187() throws WrongSizeException {
21703 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
21704
21705 final var ba = generateBa();
21706 final var biasX = ba.getElementAtIndex(0);
21707 final var biasY = ba.getElementAtIndex(1);
21708 final var biasZ = ba.getElementAtIndex(2);
21709
21710 final var ma = generateMaCommonAxis();
21711 final var sx = ma.getElementAt(0, 0);
21712 final var sy = ma.getElementAt(1, 1);
21713 final var sz = ma.getElementAt(2, 2);
21714
21715 final var randomizer = new UniformRandomizer();
21716 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21717 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21718 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21719 final var nedPosition = new NEDPosition(latitude, longitude, height);
21720 final var nedVelocity = new NEDVelocity();
21721 final var ecefPosition = new ECEFPosition();
21722 final var ecefVelocity = new ECEFVelocity();
21723 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21724 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21725 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21726 final var gravityNorm = gravity.getNormAsAcceleration();
21727
21728 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
21729 true, biasX, biasY, biasZ, sx, sy, sz, this);
21730
21731
21732 assertEquals(biasX, calibrator.getBiasX(), 0.0);
21733 assertEquals(biasY, calibrator.getBiasY(), 0.0);
21734 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21735 final var bx1 = calibrator.getBiasXAsAcceleration();
21736 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21737 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21738 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21739 calibrator.getBiasXAsAcceleration(bx2);
21740 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21741 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21742 final var by1 = calibrator.getBiasYAsAcceleration();
21743 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21744 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
21745 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21746 calibrator.getBiasYAsAcceleration(by2);
21747 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
21748 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
21749 final var bz1 = calibrator.getBiasZAsAcceleration();
21750 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
21751 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
21752 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21753 calibrator.getBiasZAsAcceleration(bz2);
21754 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
21755 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21756 final var biasTriad1 = calibrator.getBiasAsTriad();
21757 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21758 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21759 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21760 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21761 final var biasTriad2 = new AccelerationTriad();
21762 calibrator.getBiasAsTriad(biasTriad2);
21763 assertEquals(biasTriad1, biasTriad2);
21764 assertEquals(sx, calibrator.getInitialSx(), 0.0);
21765 assertEquals(sy, calibrator.getInitialSy(), 0.0);
21766 assertEquals(sz, calibrator.getInitialSz(), 0.0);
21767 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21768 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21769 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21770 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21771 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21772 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21773 final var bias1 = calibrator.getBias();
21774 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21775 final var bias2 = new double[3];
21776 calibrator.getBias(bias2);
21777 assertArrayEquals(bias1, bias2, 0.0);
21778 final var b1 = calibrator.getBiasAsMatrix();
21779 assertEquals(b1, ba);
21780 final var b2 = new Matrix(3, 1);
21781 calibrator.getBiasAsMatrix(b2);
21782 assertEquals(b1, b2);
21783 final var ma1 = calibrator.getInitialMa();
21784 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21785 final var ma2 = new Matrix(3, 3);
21786 calibrator.getInitialMa(ma2);
21787 assertEquals(ma1, ma2);
21788 assertSame(measurements, calibrator.getMeasurements());
21789 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21790 calibrator.getMeasurementType());
21791 assertFalse(calibrator.isOrderedMeasurementsRequired());
21792 assertFalse(calibrator.isQualityScoresRequired());
21793 assertTrue(calibrator.isCommonAxisUsed());
21794 assertSame(this, calibrator.getListener());
21795 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
21796 assertFalse(calibrator.isReady());
21797 assertFalse(calibrator.isRunning());
21798 assertNull(calibrator.getEstimatedMa());
21799 assertNull(calibrator.getEstimatedSx());
21800 assertNull(calibrator.getEstimatedSy());
21801 assertNull(calibrator.getEstimatedSz());
21802 assertNull(calibrator.getEstimatedMxy());
21803 assertNull(calibrator.getEstimatedMxz());
21804 assertNull(calibrator.getEstimatedMyx());
21805 assertNull(calibrator.getEstimatedMyz());
21806 assertNull(calibrator.getEstimatedMzx());
21807 assertNull(calibrator.getEstimatedMzy());
21808 assertNull(calibrator.getEstimatedCovariance());
21809 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21810 assertNotNull(calibrator.getGroundTruthGravityNorm());
21811 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21812 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21813 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21814 ABSOLUTE_ERROR));
21815 final var g = new Acceleration(0.0, AccelerationUnit.G);
21816 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21817 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21818 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21819
21820
21821 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21822
21823 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21824 invalidGravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz, this));
21825 }
21826
21827 @Test
21828 void testConstructor188() throws WrongSizeException {
21829 final var ba = generateBa();
21830 final var biasX = ba.getElementAtIndex(0);
21831 final var biasY = ba.getElementAtIndex(1);
21832 final var biasZ = ba.getElementAtIndex(2);
21833
21834 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
21835 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
21836 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
21837
21838 final var ma = generateMaCommonAxis();
21839 final var sx = ma.getElementAt(0, 0);
21840 final var sy = ma.getElementAt(1, 1);
21841 final var sz = ma.getElementAt(2, 2);
21842
21843 final var randomizer = new UniformRandomizer();
21844 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21845 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21846 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21847 final var nedPosition = new NEDPosition(latitude, longitude, height);
21848 final var nedVelocity = new NEDVelocity();
21849 final var ecefPosition = new ECEFPosition();
21850 final var ecefVelocity = new ECEFVelocity();
21851 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21852 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21853 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21854 final var gravityNorm = gravity.getNormAsAcceleration();
21855
21856 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz);
21857
21858
21859 assertEquals(biasX, calibrator.getBiasX(), 0.0);
21860 assertEquals(biasY, calibrator.getBiasY(), 0.0);
21861 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21862 final var bx1 = calibrator.getBiasXAsAcceleration();
21863 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21864 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21865 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21866 calibrator.getBiasXAsAcceleration(bx2);
21867 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21868 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21869 final var by1 = calibrator.getBiasYAsAcceleration();
21870 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21871 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
21872 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21873 calibrator.getBiasYAsAcceleration(by2);
21874 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
21875 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
21876 final var bz1 = calibrator.getBiasZAsAcceleration();
21877 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
21878 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
21879 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21880 calibrator.getBiasZAsAcceleration(bz2);
21881 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
21882 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21883 final var biasTriad1 = calibrator.getBiasAsTriad();
21884 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21885 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21886 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21887 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21888 final var biasTriad2 = new AccelerationTriad();
21889 calibrator.getBiasAsTriad(biasTriad2);
21890 assertEquals(biasTriad1, biasTriad2);
21891 assertEquals(sx, calibrator.getInitialSx(), 0.0);
21892 assertEquals(sy, calibrator.getInitialSy(), 0.0);
21893 assertEquals(sz, calibrator.getInitialSz(), 0.0);
21894 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21895 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21896 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21897 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21898 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21899 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21900 final var bias1 = calibrator.getBias();
21901 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21902 final var bias2 = new double[3];
21903 calibrator.getBias(bias2);
21904 assertArrayEquals(bias1, bias2, 0.0);
21905 final var b1 = calibrator.getBiasAsMatrix();
21906 assertEquals(b1, ba);
21907 final var b2 = new Matrix(3, 1);
21908 calibrator.getBiasAsMatrix(b2);
21909 assertEquals(b1, b2);
21910 final var ma1 = calibrator.getInitialMa();
21911 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21912 final var ma2 = new Matrix(3, 3);
21913 calibrator.getInitialMa(ma2);
21914 assertEquals(ma1, ma2);
21915 assertNull(calibrator.getMeasurements());
21916 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21917 calibrator.getMeasurementType());
21918 assertFalse(calibrator.isOrderedMeasurementsRequired());
21919 assertFalse(calibrator.isQualityScoresRequired());
21920 assertFalse(calibrator.isCommonAxisUsed());
21921 assertNull(calibrator.getListener());
21922 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
21923 assertFalse(calibrator.isReady());
21924 assertFalse(calibrator.isRunning());
21925 assertNull(calibrator.getEstimatedMa());
21926 assertNull(calibrator.getEstimatedSx());
21927 assertNull(calibrator.getEstimatedSy());
21928 assertNull(calibrator.getEstimatedSz());
21929 assertNull(calibrator.getEstimatedMxy());
21930 assertNull(calibrator.getEstimatedMxz());
21931 assertNull(calibrator.getEstimatedMyx());
21932 assertNull(calibrator.getEstimatedMyz());
21933 assertNull(calibrator.getEstimatedMzx());
21934 assertNull(calibrator.getEstimatedMzy());
21935 assertNull(calibrator.getEstimatedCovariance());
21936 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21937 assertNotNull(calibrator.getGroundTruthGravityNorm());
21938 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21939 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21940 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21941 ABSOLUTE_ERROR));
21942 final var g = new Acceleration(0.0, AccelerationUnit.G);
21943 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21944 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21945 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21946
21947
21948 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21949
21950 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21951 invalidGravityNorm, bx, by, bz, sx, sy, sz));
21952 }
21953
21954 @Test
21955 void testConstructor189() throws WrongSizeException {
21956 final var ba = generateBa();
21957 final var biasX = ba.getElementAtIndex(0);
21958 final var biasY = ba.getElementAtIndex(1);
21959 final var biasZ = ba.getElementAtIndex(2);
21960
21961 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
21962 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
21963 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
21964
21965 final var ma = generateMaCommonAxis();
21966 final var sx = ma.getElementAt(0, 0);
21967 final var sy = ma.getElementAt(1, 1);
21968 final var sz = ma.getElementAt(2, 2);
21969
21970 final var randomizer = new UniformRandomizer();
21971 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21972 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21973 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21974 final var nedPosition = new NEDPosition(latitude, longitude, height);
21975 final var nedVelocity = new NEDVelocity();
21976 final var ecefPosition = new ECEFPosition();
21977 final var ecefVelocity = new ECEFVelocity();
21978 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21979 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21980 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21981 final var gravityNorm = gravity.getNormAsAcceleration();
21982
21983 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz,
21984 this);
21985
21986
21987 assertEquals(biasX, calibrator.getBiasX(), 0.0);
21988 assertEquals(biasY, calibrator.getBiasY(), 0.0);
21989 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21990 final var bx1 = calibrator.getBiasXAsAcceleration();
21991 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21992 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21993 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21994 calibrator.getBiasXAsAcceleration(bx2);
21995 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21996 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21997 final var by1 = calibrator.getBiasYAsAcceleration();
21998 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21999 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22000 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22001 calibrator.getBiasYAsAcceleration(by2);
22002 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22003 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22004 final var bz1 = calibrator.getBiasZAsAcceleration();
22005 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22006 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22007 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22008 calibrator.getBiasZAsAcceleration(bz2);
22009 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22010 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22011 final var biasTriad1 = calibrator.getBiasAsTriad();
22012 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22013 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22014 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22015 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22016 final var biasTriad2 = new AccelerationTriad();
22017 calibrator.getBiasAsTriad(biasTriad2);
22018 assertEquals(biasTriad1, biasTriad2);
22019 assertEquals(sx, calibrator.getInitialSx(), 0.0);
22020 assertEquals(sy, calibrator.getInitialSy(), 0.0);
22021 assertEquals(sz, calibrator.getInitialSz(), 0.0);
22022 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
22023 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
22024 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
22025 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
22026 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
22027 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
22028 final var bias1 = calibrator.getBias();
22029 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22030 final var bias2 = new double[3];
22031 calibrator.getBias(bias2);
22032 assertArrayEquals(bias1, bias2, 0.0);
22033 final var b1 = calibrator.getBiasAsMatrix();
22034 assertEquals(b1, ba);
22035 final var b2 = new Matrix(3, 1);
22036 calibrator.getBiasAsMatrix(b2);
22037 assertEquals(b1, b2);
22038 final var ma1 = calibrator.getInitialMa();
22039 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22040 final var ma2 = new Matrix(3, 3);
22041 calibrator.getInitialMa(ma2);
22042 assertEquals(ma1, ma2);
22043 assertNull(calibrator.getMeasurements());
22044 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22045 calibrator.getMeasurementType());
22046 assertFalse(calibrator.isOrderedMeasurementsRequired());
22047 assertFalse(calibrator.isQualityScoresRequired());
22048 assertFalse(calibrator.isCommonAxisUsed());
22049 assertSame(this, calibrator.getListener());
22050 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
22051 assertFalse(calibrator.isReady());
22052 assertFalse(calibrator.isRunning());
22053 assertNull(calibrator.getEstimatedMa());
22054 assertNull(calibrator.getEstimatedSx());
22055 assertNull(calibrator.getEstimatedSy());
22056 assertNull(calibrator.getEstimatedSz());
22057 assertNull(calibrator.getEstimatedMxy());
22058 assertNull(calibrator.getEstimatedMxz());
22059 assertNull(calibrator.getEstimatedMyx());
22060 assertNull(calibrator.getEstimatedMyz());
22061 assertNull(calibrator.getEstimatedMzx());
22062 assertNull(calibrator.getEstimatedMzy());
22063 assertNull(calibrator.getEstimatedCovariance());
22064 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22065 assertNotNull(calibrator.getGroundTruthGravityNorm());
22066 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22067 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22068 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22069 ABSOLUTE_ERROR));
22070 final var g = new Acceleration(0.0, AccelerationUnit.G);
22071 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22072 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22073 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22074
22075
22076 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22077
22078 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22079 invalidGravityNorm, bx, by, bz, sx, sy, sz, this));
22080 }
22081
22082 @Test
22083 void testConstructor190() throws WrongSizeException {
22084 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
22085
22086 final var ba = generateBa();
22087 final var biasX = ba.getElementAtIndex(0);
22088 final var biasY = ba.getElementAtIndex(1);
22089 final var biasZ = ba.getElementAtIndex(2);
22090
22091 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22092 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22093 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22094
22095 final var ma = generateMaCommonAxis();
22096 final var sx = ma.getElementAt(0, 0);
22097 final var sy = ma.getElementAt(1, 1);
22098 final var sz = ma.getElementAt(2, 2);
22099
22100 final var randomizer = new UniformRandomizer();
22101 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22102 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22103 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22104 final var nedPosition = new NEDPosition(latitude, longitude, height);
22105 final var nedVelocity = new NEDVelocity();
22106 final var ecefPosition = new ECEFPosition();
22107 final var ecefVelocity = new ECEFVelocity();
22108 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
22109 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22110 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22111 final var gravityNorm = gravity.getNormAsAcceleration();
22112
22113 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
22114 sx, sy, sz);
22115
22116
22117 assertEquals(biasX, calibrator.getBiasX(), 0.0);
22118 assertEquals(biasY, calibrator.getBiasY(), 0.0);
22119 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
22120 final var bx1 = calibrator.getBiasXAsAcceleration();
22121 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
22122 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
22123 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22124 calibrator.getBiasXAsAcceleration(bx2);
22125 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
22126 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
22127 final var by1 = calibrator.getBiasYAsAcceleration();
22128 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
22129 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22130 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22131 calibrator.getBiasYAsAcceleration(by2);
22132 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22133 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22134 final var bz1 = calibrator.getBiasZAsAcceleration();
22135 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22136 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22137 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22138 calibrator.getBiasZAsAcceleration(bz2);
22139 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22140 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22141 final var biasTriad1 = calibrator.getBiasAsTriad();
22142 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22143 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22144 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22145 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22146 final var biasTriad2 = new AccelerationTriad();
22147 calibrator.getBiasAsTriad(biasTriad2);
22148 assertEquals(biasTriad1, biasTriad2);
22149 assertEquals(sx, calibrator.getInitialSx(), 0.0);
22150 assertEquals(sy, calibrator.getInitialSy(), 0.0);
22151 assertEquals(sz, calibrator.getInitialSz(), 0.0);
22152 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
22153 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
22154 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
22155 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
22156 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
22157 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
22158 final var bias1 = calibrator.getBias();
22159 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22160 final var bias2 = new double[3];
22161 calibrator.getBias(bias2);
22162 assertArrayEquals(bias1, bias2, 0.0);
22163 final var b1 = calibrator.getBiasAsMatrix();
22164 assertEquals(b1, ba);
22165 final var b2 = new Matrix(3, 1);
22166 calibrator.getBiasAsMatrix(b2);
22167 assertEquals(b1, b2);
22168 final var ma1 = calibrator.getInitialMa();
22169 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22170 final var ma2 = new Matrix(3, 3);
22171 calibrator.getInitialMa(ma2);
22172 assertEquals(ma1, ma2);
22173 assertSame(measurements, calibrator.getMeasurements());
22174 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22175 calibrator.getMeasurementType());
22176 assertFalse(calibrator.isOrderedMeasurementsRequired());
22177 assertFalse(calibrator.isQualityScoresRequired());
22178 assertFalse(calibrator.isCommonAxisUsed());
22179 assertNull(calibrator.getListener());
22180 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
22181 assertFalse(calibrator.isReady());
22182 assertFalse(calibrator.isRunning());
22183 assertNull(calibrator.getEstimatedMa());
22184 assertNull(calibrator.getEstimatedSx());
22185 assertNull(calibrator.getEstimatedSy());
22186 assertNull(calibrator.getEstimatedSz());
22187 assertNull(calibrator.getEstimatedMxy());
22188 assertNull(calibrator.getEstimatedMxz());
22189 assertNull(calibrator.getEstimatedMyx());
22190 assertNull(calibrator.getEstimatedMyz());
22191 assertNull(calibrator.getEstimatedMzx());
22192 assertNull(calibrator.getEstimatedMzy());
22193 assertNull(calibrator.getEstimatedCovariance());
22194 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22195 assertNotNull(calibrator.getGroundTruthGravityNorm());
22196 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22197 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22198 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22199 ABSOLUTE_ERROR));
22200 final var g = new Acceleration(0.0, AccelerationUnit.G);
22201 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22202 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22203 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22204
22205
22206 final var invalidGravityNorm = new Acceleration(
22207 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22208
22209 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22210 invalidGravityNorm, measurements, bx, by, bz, sx, sy, sz));
22211 }
22212
22213 @Test
22214 void testConstructor191() throws WrongSizeException {
22215 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
22216
22217 final var ba = generateBa();
22218 final var biasX = ba.getElementAtIndex(0);
22219 final var biasY = ba.getElementAtIndex(1);
22220 final var biasZ = ba.getElementAtIndex(2);
22221
22222 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22223 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22224 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22225
22226 final var ma = generateMaCommonAxis();
22227 final var sx = ma.getElementAt(0, 0);
22228 final var sy = ma.getElementAt(1, 1);
22229 final var sz = ma.getElementAt(2, 2);
22230
22231 final var randomizer = new UniformRandomizer();
22232 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22233 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22234 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22235 final var nedPosition = new NEDPosition(latitude, longitude, height);
22236 final var nedVelocity = new NEDVelocity();
22237 final var ecefPosition = new ECEFPosition();
22238 final var ecefVelocity = new ECEFVelocity();
22239 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
22240 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22241 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22242 final var gravityNorm = gravity.getNormAsAcceleration();
22243
22244 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
22245 sx, sy, sz, this);
22246
22247
22248 assertEquals(biasX, calibrator.getBiasX(), 0.0);
22249 assertEquals(biasY, calibrator.getBiasY(), 0.0);
22250 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
22251 final var bx1 = calibrator.getBiasXAsAcceleration();
22252 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
22253 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
22254 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22255 calibrator.getBiasXAsAcceleration(bx2);
22256 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
22257 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
22258 final var by1 = calibrator.getBiasYAsAcceleration();
22259 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
22260 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22261 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22262 calibrator.getBiasYAsAcceleration(by2);
22263 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22264 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22265 final var bz1 = calibrator.getBiasZAsAcceleration();
22266 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22267 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22268 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22269 calibrator.getBiasZAsAcceleration(bz2);
22270 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22271 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22272 final var biasTriad1 = calibrator.getBiasAsTriad();
22273 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22274 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22275 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22276 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22277 final var biasTriad2 = new AccelerationTriad();
22278 calibrator.getBiasAsTriad(biasTriad2);
22279 assertEquals(biasTriad1, biasTriad2);
22280 assertEquals(sx, calibrator.getInitialSx(), 0.0);
22281 assertEquals(sy, calibrator.getInitialSy(), 0.0);
22282 assertEquals(sz, calibrator.getInitialSz(), 0.0);
22283 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
22284 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
22285 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
22286 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
22287 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
22288 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
22289 final var bias1 = calibrator.getBias();
22290 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22291 final var bias2 = new double[3];
22292 calibrator.getBias(bias2);
22293 assertArrayEquals(bias1, bias2, 0.0);
22294 final var b1 = calibrator.getBiasAsMatrix();
22295 assertEquals(b1, ba);
22296 final var b2 = new Matrix(3, 1);
22297 calibrator.getBiasAsMatrix(b2);
22298 assertEquals(b1, b2);
22299 final var ma1 = calibrator.getInitialMa();
22300 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22301 final var ma2 = new Matrix(3, 3);
22302 calibrator.getInitialMa(ma2);
22303 assertEquals(ma1, ma2);
22304 assertSame(measurements, calibrator.getMeasurements());
22305 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22306 calibrator.getMeasurementType());
22307 assertFalse(calibrator.isOrderedMeasurementsRequired());
22308 assertFalse(calibrator.isQualityScoresRequired());
22309 assertFalse(calibrator.isCommonAxisUsed());
22310 assertSame(this, calibrator.getListener());
22311 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
22312 assertFalse(calibrator.isReady());
22313 assertFalse(calibrator.isRunning());
22314 assertNull(calibrator.getEstimatedMa());
22315 assertNull(calibrator.getEstimatedSx());
22316 assertNull(calibrator.getEstimatedSy());
22317 assertNull(calibrator.getEstimatedSz());
22318 assertNull(calibrator.getEstimatedMxy());
22319 assertNull(calibrator.getEstimatedMxz());
22320 assertNull(calibrator.getEstimatedMyx());
22321 assertNull(calibrator.getEstimatedMyz());
22322 assertNull(calibrator.getEstimatedMzx());
22323 assertNull(calibrator.getEstimatedMzy());
22324 assertNull(calibrator.getEstimatedCovariance());
22325 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22326 assertNotNull(calibrator.getGroundTruthGravityNorm());
22327 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22328 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22329 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22330 ABSOLUTE_ERROR));
22331 final var g = new Acceleration(0.0, AccelerationUnit.G);
22332 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22333 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22334 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22335
22336
22337 final var invalidGravityNorm = new Acceleration(
22338 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22339
22340 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22341 invalidGravityNorm, measurements, bx, by, bz, sx, sy, sz, this));
22342 }
22343
22344 @Test
22345 void testConstructor192() throws WrongSizeException {
22346 final var ba = generateBa();
22347 final var biasX = ba.getElementAtIndex(0);
22348 final var biasY = ba.getElementAtIndex(1);
22349 final var biasZ = ba.getElementAtIndex(2);
22350
22351 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22352 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22353 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22354
22355 final var ma = generateMaCommonAxis();
22356 final var sx = ma.getElementAt(0, 0);
22357 final var sy = ma.getElementAt(1, 1);
22358 final var sz = ma.getElementAt(2, 2);
22359
22360 final var randomizer = new UniformRandomizer();
22361 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22362 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22363 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22364 final var nedPosition = new NEDPosition(latitude, longitude, height);
22365 final var nedVelocity = new NEDVelocity();
22366 final var ecefPosition = new ECEFPosition();
22367 final var ecefVelocity = new ECEFVelocity();
22368 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
22369 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22370 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22371 final var gravityNorm = gravity.getNormAsAcceleration();
22372
22373 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
22374 bx, by, bz, sx, sy, sz);
22375
22376
22377 assertEquals(biasX, calibrator.getBiasX(), 0.0);
22378 assertEquals(biasY, calibrator.getBiasY(), 0.0);
22379 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
22380 final var bx1 = calibrator.getBiasXAsAcceleration();
22381 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
22382 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
22383 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22384 calibrator.getBiasXAsAcceleration(bx2);
22385 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
22386 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
22387 final var by1 = calibrator.getBiasYAsAcceleration();
22388 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
22389 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22390 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22391 calibrator.getBiasYAsAcceleration(by2);
22392 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22393 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22394 final var bz1 = calibrator.getBiasZAsAcceleration();
22395 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22396 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22397 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22398 calibrator.getBiasZAsAcceleration(bz2);
22399 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22400 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22401 final var biasTriad1 = calibrator.getBiasAsTriad();
22402 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22403 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22404 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22405 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22406 final var biasTriad2 = new AccelerationTriad();
22407 calibrator.getBiasAsTriad(biasTriad2);
22408 assertEquals(biasTriad1, biasTriad2);
22409 assertEquals(sx, calibrator.getInitialSx(), 0.0);
22410 assertEquals(sy, calibrator.getInitialSy(), 0.0);
22411 assertEquals(sz, calibrator.getInitialSz(), 0.0);
22412 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
22413 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
22414 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
22415 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
22416 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
22417 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
22418 final var bias1 = calibrator.getBias();
22419 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22420 final var bias2 = new double[3];
22421 calibrator.getBias(bias2);
22422 assertArrayEquals(bias1, bias2, 0.0);
22423 final var b1 = calibrator.getBiasAsMatrix();
22424 assertEquals(b1, ba);
22425 final var b2 = new Matrix(3, 1);
22426 calibrator.getBiasAsMatrix(b2);
22427 assertEquals(b1, b2);
22428 final var ma1 = calibrator.getInitialMa();
22429 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22430 final var ma2 = new Matrix(3, 3);
22431 calibrator.getInitialMa(ma2);
22432 assertEquals(ma1, ma2);
22433 assertNull(calibrator.getMeasurements());
22434 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22435 calibrator.getMeasurementType());
22436 assertFalse(calibrator.isOrderedMeasurementsRequired());
22437 assertFalse(calibrator.isQualityScoresRequired());
22438 assertTrue(calibrator.isCommonAxisUsed());
22439 assertNull(calibrator.getListener());
22440 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
22441 assertFalse(calibrator.isReady());
22442 assertFalse(calibrator.isRunning());
22443 assertNull(calibrator.getEstimatedMa());
22444 assertNull(calibrator.getEstimatedSx());
22445 assertNull(calibrator.getEstimatedSy());
22446 assertNull(calibrator.getEstimatedSz());
22447 assertNull(calibrator.getEstimatedMxy());
22448 assertNull(calibrator.getEstimatedMxz());
22449 assertNull(calibrator.getEstimatedMyx());
22450 assertNull(calibrator.getEstimatedMyz());
22451 assertNull(calibrator.getEstimatedMzx());
22452 assertNull(calibrator.getEstimatedMzy());
22453 assertNull(calibrator.getEstimatedCovariance());
22454 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22455 assertNotNull(calibrator.getGroundTruthGravityNorm());
22456 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22457 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22458 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22459 ABSOLUTE_ERROR));
22460 final var g = new Acceleration(0.0, AccelerationUnit.G);
22461 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22462 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22463 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22464
22465
22466 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22467
22468 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22469 invalidGravityNorm, true, bx, by, bz, sx, sy, sz));
22470 }
22471
22472 @Test
22473 void testConstructor193() throws WrongSizeException {
22474 final var ba = generateBa();
22475 final var biasX = ba.getElementAtIndex(0);
22476 final var biasY = ba.getElementAtIndex(1);
22477 final var biasZ = ba.getElementAtIndex(2);
22478
22479 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22480 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22481 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22482
22483 final var ma = generateMaCommonAxis();
22484 final var sx = ma.getElementAt(0, 0);
22485 final var sy = ma.getElementAt(1, 1);
22486 final var sz = ma.getElementAt(2, 2);
22487
22488 final var randomizer = new UniformRandomizer();
22489 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22490 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22491 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22492 final var nedPosition = new NEDPosition(latitude, longitude, height);
22493 final var nedVelocity = new NEDVelocity();
22494 final var ecefPosition = new ECEFPosition();
22495 final var ecefVelocity = new ECEFVelocity();
22496 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
22497 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22498 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22499 final var gravityNorm = gravity.getNormAsAcceleration();
22500
22501 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
22502 bx, by, bz, sx, sy, sz, this);
22503
22504
22505 assertEquals(biasX, calibrator.getBiasX(), 0.0);
22506 assertEquals(biasY, calibrator.getBiasY(), 0.0);
22507 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
22508 final var bx1 = calibrator.getBiasXAsAcceleration();
22509 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
22510 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
22511 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22512 calibrator.getBiasXAsAcceleration(bx2);
22513 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
22514 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
22515 final var by1 = calibrator.getBiasYAsAcceleration();
22516 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
22517 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22518 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22519 calibrator.getBiasYAsAcceleration(by2);
22520 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22521 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22522 final var bz1 = calibrator.getBiasZAsAcceleration();
22523 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22524 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22525 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22526 calibrator.getBiasZAsAcceleration(bz2);
22527 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22528 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22529 final var biasTriad1 = calibrator.getBiasAsTriad();
22530 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22531 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22532 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22533 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22534 final var biasTriad2 = new AccelerationTriad();
22535 calibrator.getBiasAsTriad(biasTriad2);
22536 assertEquals(biasTriad1, biasTriad2);
22537 assertEquals(sx, calibrator.getInitialSx(), 0.0);
22538 assertEquals(sy, calibrator.getInitialSy(), 0.0);
22539 assertEquals(sz, calibrator.getInitialSz(), 0.0);
22540 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
22541 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
22542 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
22543 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
22544 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
22545 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
22546 final var bias1 = calibrator.getBias();
22547 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22548 final var bias2 = new double[3];
22549 calibrator.getBias(bias2);
22550 assertArrayEquals(bias1, bias2, 0.0);
22551 final var b1 = calibrator.getBiasAsMatrix();
22552 assertEquals(b1, ba);
22553 final var b2 = new Matrix(3, 1);
22554 calibrator.getBiasAsMatrix(b2);
22555 assertEquals(b1, b2);
22556 final var ma1 = calibrator.getInitialMa();
22557 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22558 final var ma2 = new Matrix(3, 3);
22559 calibrator.getInitialMa(ma2);
22560 assertEquals(ma1, ma2);
22561 assertNull(calibrator.getMeasurements());
22562 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22563 calibrator.getMeasurementType());
22564 assertFalse(calibrator.isOrderedMeasurementsRequired());
22565 assertFalse(calibrator.isQualityScoresRequired());
22566 assertTrue(calibrator.isCommonAxisUsed());
22567 assertSame(this, calibrator.getListener());
22568 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
22569 assertFalse(calibrator.isReady());
22570 assertFalse(calibrator.isRunning());
22571 assertNull(calibrator.getEstimatedMa());
22572 assertNull(calibrator.getEstimatedSx());
22573 assertNull(calibrator.getEstimatedSy());
22574 assertNull(calibrator.getEstimatedSz());
22575 assertNull(calibrator.getEstimatedMxy());
22576 assertNull(calibrator.getEstimatedMxz());
22577 assertNull(calibrator.getEstimatedMyx());
22578 assertNull(calibrator.getEstimatedMyz());
22579 assertNull(calibrator.getEstimatedMzx());
22580 assertNull(calibrator.getEstimatedMzy());
22581 assertNull(calibrator.getEstimatedCovariance());
22582 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22583 assertNotNull(calibrator.getGroundTruthGravityNorm());
22584 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22585 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22586 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22587 ABSOLUTE_ERROR));
22588 final var g = new Acceleration(0.0, AccelerationUnit.G);
22589 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22590 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22591 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22592
22593
22594 final var invalidGravityNorm = new Acceleration(
22595 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22596
22597 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22598 invalidGravityNorm, true, bx, by, bz, sx, sy, sz, this));
22599 }
22600
22601 @Test
22602 void testConstructor194() throws WrongSizeException {
22603 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
22604
22605 final var ba = generateBa();
22606 final var biasX = ba.getElementAtIndex(0);
22607 final var biasY = ba.getElementAtIndex(1);
22608 final var biasZ = ba.getElementAtIndex(2);
22609
22610 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22611 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22612 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22613
22614 final var ma = generateMaCommonAxis();
22615 final var sx = ma.getElementAt(0, 0);
22616 final var sy = ma.getElementAt(1, 1);
22617 final var sz = ma.getElementAt(2, 2);
22618
22619 final var randomizer = new UniformRandomizer();
22620 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22621 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22622 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22623 final var nedPosition = new NEDPosition(latitude, longitude, height);
22624 final var nedVelocity = new NEDVelocity();
22625 final var ecefPosition = new ECEFPosition();
22626 final var ecefVelocity = new ECEFVelocity();
22627 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
22628 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22629 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22630 final var gravityNorm = gravity.getNormAsAcceleration();
22631
22632 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
22633 true, bx, by, bz, sx, sy, sz);
22634
22635
22636 assertEquals(biasX, calibrator.getBiasX(), 0.0);
22637 assertEquals(biasY, calibrator.getBiasY(), 0.0);
22638 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
22639 final var bx1 = calibrator.getBiasXAsAcceleration();
22640 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
22641 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
22642 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22643 calibrator.getBiasXAsAcceleration(bx2);
22644 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
22645 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
22646 final var by1 = calibrator.getBiasYAsAcceleration();
22647 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
22648 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22649 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22650 calibrator.getBiasYAsAcceleration(by2);
22651 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22652 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22653 final var bz1 = calibrator.getBiasZAsAcceleration();
22654 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22655 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22656 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22657 calibrator.getBiasZAsAcceleration(bz2);
22658 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22659 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22660 final var biasTriad1 = calibrator.getBiasAsTriad();
22661 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22662 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22663 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22664 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22665 final var biasTriad2 = new AccelerationTriad();
22666 calibrator.getBiasAsTriad(biasTriad2);
22667 assertEquals(biasTriad1, biasTriad2);
22668 assertEquals(sx, calibrator.getInitialSx(), 0.0);
22669 assertEquals(sy, calibrator.getInitialSy(), 0.0);
22670 assertEquals(sz, calibrator.getInitialSz(), 0.0);
22671 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
22672 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
22673 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
22674 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
22675 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
22676 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
22677 final var bias1 = calibrator.getBias();
22678 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22679 final var bias2 = new double[3];
22680 calibrator.getBias(bias2);
22681 assertArrayEquals(bias1, bias2, 0.0);
22682 final var b1 = calibrator.getBiasAsMatrix();
22683 assertEquals(b1, ba);
22684 final var b2 = new Matrix(3, 1);
22685 calibrator.getBiasAsMatrix(b2);
22686 assertEquals(b1, b2);
22687 final var ma1 = calibrator.getInitialMa();
22688 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22689 final var ma2 = new Matrix(3, 3);
22690 calibrator.getInitialMa(ma2);
22691 assertEquals(ma1, ma2);
22692 assertSame(measurements, calibrator.getMeasurements());
22693 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22694 calibrator.getMeasurementType());
22695 assertFalse(calibrator.isOrderedMeasurementsRequired());
22696 assertFalse(calibrator.isQualityScoresRequired());
22697 assertTrue(calibrator.isCommonAxisUsed());
22698 assertNull(calibrator.getListener());
22699 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
22700 assertFalse(calibrator.isReady());
22701 assertFalse(calibrator.isRunning());
22702 assertNull(calibrator.getEstimatedMa());
22703 assertNull(calibrator.getEstimatedSx());
22704 assertNull(calibrator.getEstimatedSy());
22705 assertNull(calibrator.getEstimatedSz());
22706 assertNull(calibrator.getEstimatedMxy());
22707 assertNull(calibrator.getEstimatedMxz());
22708 assertNull(calibrator.getEstimatedMyx());
22709 assertNull(calibrator.getEstimatedMyz());
22710 assertNull(calibrator.getEstimatedMzx());
22711 assertNull(calibrator.getEstimatedMzy());
22712 assertNull(calibrator.getEstimatedCovariance());
22713 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22714 assertNotNull(calibrator.getGroundTruthGravityNorm());
22715 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22716 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22717 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22718 ABSOLUTE_ERROR));
22719 final var g = new Acceleration(0.0, AccelerationUnit.G);
22720 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22721 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22722 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22723
22724
22725 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22726
22727 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22728 invalidGravityNorm, measurements, true, bx, by, bz, sx, sy, sz));
22729 }
22730
22731 @Test
22732 void testConstructor195() throws WrongSizeException {
22733 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
22734
22735 final var ba = generateBa();
22736 final var biasX = ba.getElementAtIndex(0);
22737 final var biasY = ba.getElementAtIndex(1);
22738 final var biasZ = ba.getElementAtIndex(2);
22739
22740 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22741 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22742 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22743
22744 final var ma = generateMaCommonAxis();
22745 final var sx = ma.getElementAt(0, 0);
22746 final var sy = ma.getElementAt(1, 1);
22747 final var sz = ma.getElementAt(2, 2);
22748
22749 final var randomizer = new UniformRandomizer();
22750 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22751 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22752 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22753 final var nedPosition = new NEDPosition(latitude, longitude, height);
22754 final var nedVelocity = new NEDVelocity();
22755 final var ecefPosition = new ECEFPosition();
22756 final var ecefVelocity = new ECEFVelocity();
22757 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
22758 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22759 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22760 final var gravityNorm = gravity.getNormAsAcceleration();
22761
22762 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
22763 true, bx, by, bz, sx, sy, sz, this);
22764
22765
22766 assertEquals(biasX, calibrator.getBiasX(), 0.0);
22767 assertEquals(biasY, calibrator.getBiasY(), 0.0);
22768 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
22769 final var bx1 = calibrator.getBiasXAsAcceleration();
22770 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
22771 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
22772 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22773 calibrator.getBiasXAsAcceleration(bx2);
22774 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
22775 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
22776 final var by1 = calibrator.getBiasYAsAcceleration();
22777 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
22778 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22779 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22780 calibrator.getBiasYAsAcceleration(by2);
22781 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22782 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22783 final var bz1 = calibrator.getBiasZAsAcceleration();
22784 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22785 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22786 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22787 calibrator.getBiasZAsAcceleration(bz2);
22788 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22789 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22790 final var biasTriad1 = calibrator.getBiasAsTriad();
22791 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22792 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22793 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22794 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22795 final var biasTriad2 = new AccelerationTriad();
22796 calibrator.getBiasAsTriad(biasTriad2);
22797 assertEquals(biasTriad1, biasTriad2);
22798 assertEquals(sx, calibrator.getInitialSx(), 0.0);
22799 assertEquals(sy, calibrator.getInitialSy(), 0.0);
22800 assertEquals(sz, calibrator.getInitialSz(), 0.0);
22801 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
22802 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
22803 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
22804 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
22805 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
22806 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
22807 final var bias1 = calibrator.getBias();
22808 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22809 final var bias2 = new double[3];
22810 calibrator.getBias(bias2);
22811 assertArrayEquals(bias1, bias2, 0.0);
22812 final var b1 = calibrator.getBiasAsMatrix();
22813 assertEquals(b1, ba);
22814 final var b2 = new Matrix(3, 1);
22815 calibrator.getBiasAsMatrix(b2);
22816 assertEquals(b1, b2);
22817 final var ma1 = calibrator.getInitialMa();
22818 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22819 final var ma2 = new Matrix(3, 3);
22820 calibrator.getInitialMa(ma2);
22821 assertEquals(ma1, ma2);
22822 assertSame(measurements, calibrator.getMeasurements());
22823 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22824 calibrator.getMeasurementType());
22825 assertFalse(calibrator.isOrderedMeasurementsRequired());
22826 assertFalse(calibrator.isQualityScoresRequired());
22827 assertTrue(calibrator.isCommonAxisUsed());
22828 assertSame(this, calibrator.getListener());
22829 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
22830 assertFalse(calibrator.isReady());
22831 assertFalse(calibrator.isRunning());
22832 assertNull(calibrator.getEstimatedMa());
22833 assertNull(calibrator.getEstimatedSx());
22834 assertNull(calibrator.getEstimatedSy());
22835 assertNull(calibrator.getEstimatedSz());
22836 assertNull(calibrator.getEstimatedMxy());
22837 assertNull(calibrator.getEstimatedMxz());
22838 assertNull(calibrator.getEstimatedMyx());
22839 assertNull(calibrator.getEstimatedMyz());
22840 assertNull(calibrator.getEstimatedMzx());
22841 assertNull(calibrator.getEstimatedMzy());
22842 assertNull(calibrator.getEstimatedCovariance());
22843 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22844 assertNotNull(calibrator.getGroundTruthGravityNorm());
22845 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22846 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22847 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22848 ABSOLUTE_ERROR));
22849 final var g = new Acceleration(0.0, AccelerationUnit.G);
22850 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22851 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22852 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22853
22854
22855 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22856
22857 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22858 invalidGravityNorm, measurements, true, bx, by, bz, sx, sy, sz, this));
22859 }
22860
22861 @Test
22862 void testConstructor196() throws WrongSizeException {
22863 final var ba = generateBa();
22864 final var biasX = ba.getElementAtIndex(0);
22865 final var biasY = ba.getElementAtIndex(1);
22866 final var biasZ = ba.getElementAtIndex(2);
22867
22868 final var ma = generateMaCommonAxis();
22869 final var sx = ma.getElementAt(0, 0);
22870 final var sy = ma.getElementAt(1, 1);
22871 final var sz = ma.getElementAt(2, 2);
22872 final var mxy = ma.getElementAt(0, 1);
22873 final var mxz = ma.getElementAt(0, 2);
22874 final var myx = ma.getElementAt(1, 0);
22875 final var myz = ma.getElementAt(1, 2);
22876 final var mzx = ma.getElementAt(2, 0);
22877 final var mzy = ma.getElementAt(2, 1);
22878
22879 final var randomizer = new UniformRandomizer();
22880 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22881 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22882 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22883 final var nedPosition = new NEDPosition(latitude, longitude, height);
22884 final var nedVelocity = new NEDVelocity();
22885 final var ecefPosition = new ECEFPosition();
22886 final var ecefVelocity = new ECEFVelocity();
22887 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
22888 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22889 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22890 final var gravityNorm = gravity.getNormAsAcceleration();
22891
22892 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ,
22893 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
22894
22895
22896 assertEquals(biasX, calibrator.getBiasX(), 0.0);
22897 assertEquals(biasY, calibrator.getBiasY(), 0.0);
22898 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
22899 final var bx1 = calibrator.getBiasXAsAcceleration();
22900 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
22901 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
22902 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22903 calibrator.getBiasXAsAcceleration(bx2);
22904 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
22905 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
22906 final var by1 = calibrator.getBiasYAsAcceleration();
22907 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
22908 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22909 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22910 calibrator.getBiasYAsAcceleration(by2);
22911 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22912 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22913 final var bz1 = calibrator.getBiasZAsAcceleration();
22914 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22915 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22916 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22917 calibrator.getBiasZAsAcceleration(bz2);
22918 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22919 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22920 final var biasTriad1 = calibrator.getBiasAsTriad();
22921 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22922 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22923 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22924 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22925 final var biasTriad2 = new AccelerationTriad();
22926 calibrator.getBiasAsTriad(biasTriad2);
22927 assertEquals(biasTriad1, biasTriad2);
22928 assertEquals(sx, calibrator.getInitialSx(), 0.0);
22929 assertEquals(sy, calibrator.getInitialSy(), 0.0);
22930 assertEquals(sz, calibrator.getInitialSz(), 0.0);
22931 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
22932 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
22933 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
22934 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
22935 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
22936 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
22937 final var bias1 = calibrator.getBias();
22938 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22939 final var bias2 = new double[3];
22940 calibrator.getBias(bias2);
22941 assertArrayEquals(bias1, bias2, 0.0);
22942 final var b1 = calibrator.getBiasAsMatrix();
22943 assertEquals(b1, ba);
22944 final var b2 = new Matrix(3, 1);
22945 calibrator.getBiasAsMatrix(b2);
22946 assertEquals(b1, b2);
22947 final var ma1 = new Matrix(3, 3);
22948 ma1.setSubmatrix(0, 0, 2, 2,
22949 new double[]{sx, myx, mzx,
22950 mxy, sy, mzy,
22951 mxz, myz, sz});
22952 assertEquals(calibrator.getInitialMa(), ma1);
22953 final var ma2 = new Matrix(3, 3);
22954 calibrator.getInitialMa(ma2);
22955 assertEquals(ma1, ma2);
22956 assertNull(calibrator.getMeasurements());
22957 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22958 calibrator.getMeasurementType());
22959 assertFalse(calibrator.isOrderedMeasurementsRequired());
22960 assertFalse(calibrator.isQualityScoresRequired());
22961 assertFalse(calibrator.isCommonAxisUsed());
22962 assertNull(calibrator.getListener());
22963 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
22964 assertFalse(calibrator.isReady());
22965 assertFalse(calibrator.isRunning());
22966 assertNull(calibrator.getEstimatedMa());
22967 assertNull(calibrator.getEstimatedSx());
22968 assertNull(calibrator.getEstimatedSy());
22969 assertNull(calibrator.getEstimatedSz());
22970 assertNull(calibrator.getEstimatedMxy());
22971 assertNull(calibrator.getEstimatedMxz());
22972 assertNull(calibrator.getEstimatedMyx());
22973 assertNull(calibrator.getEstimatedMyz());
22974 assertNull(calibrator.getEstimatedMzx());
22975 assertNull(calibrator.getEstimatedMzy());
22976 assertNull(calibrator.getEstimatedCovariance());
22977 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22978 assertNotNull(calibrator.getGroundTruthGravityNorm());
22979 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22980 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22981 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22982 ABSOLUTE_ERROR));
22983 final var g = new Acceleration(0.0, AccelerationUnit.G);
22984 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22985 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22986 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22987
22988
22989 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22990
22991 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22992 invalidGravityNorm, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
22993 }
22994
22995 @Test
22996 void testConstructor197() throws WrongSizeException {
22997 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
22998
22999 final var ba = generateBa();
23000 final var biasX = ba.getElementAtIndex(0);
23001 final var biasY = ba.getElementAtIndex(1);
23002 final var biasZ = ba.getElementAtIndex(2);
23003
23004 final var ma = generateMaCommonAxis();
23005 final var sx = ma.getElementAt(0, 0);
23006 final var sy = ma.getElementAt(1, 1);
23007 final var sz = ma.getElementAt(2, 2);
23008 final var mxy = ma.getElementAt(0, 1);
23009 final var mxz = ma.getElementAt(0, 2);
23010 final var myx = ma.getElementAt(1, 0);
23011 final var myz = ma.getElementAt(1, 2);
23012 final var mzx = ma.getElementAt(2, 0);
23013 final var mzy = ma.getElementAt(2, 1);
23014
23015 final var randomizer = new UniformRandomizer();
23016 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23017 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23018 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23019 final var nedPosition = new NEDPosition(latitude, longitude, height);
23020 final var nedVelocity = new NEDVelocity();
23021 final var ecefPosition = new ECEFPosition();
23022 final var ecefVelocity = new ECEFVelocity();
23023 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23024 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23025 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23026 final var gravityNorm = gravity.getNormAsAcceleration();
23027
23028 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
23029 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
23030
23031
23032 assertEquals(biasX, calibrator.getBiasX(), 0.0);
23033 assertEquals(biasY, calibrator.getBiasY(), 0.0);
23034 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23035 final var bx1 = calibrator.getBiasXAsAcceleration();
23036 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23037 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23038 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23039 calibrator.getBiasXAsAcceleration(bx2);
23040 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23041 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23042 final var by1 = calibrator.getBiasYAsAcceleration();
23043 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23044 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23045 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23046 calibrator.getBiasYAsAcceleration(by2);
23047 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
23048 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
23049 final var bz1 = calibrator.getBiasZAsAcceleration();
23050 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
23051 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
23052 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23053 calibrator.getBiasZAsAcceleration(bz2);
23054 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
23055 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
23056 final var biasTriad1 = calibrator.getBiasAsTriad();
23057 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
23058 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
23059 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
23060 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
23061 final var biasTriad2 = new AccelerationTriad();
23062 calibrator.getBiasAsTriad(biasTriad2);
23063 assertEquals(biasTriad1, biasTriad2);
23064 assertEquals(sx, calibrator.getInitialSx(), 0.0);
23065 assertEquals(sy, calibrator.getInitialSy(), 0.0);
23066 assertEquals(sz, calibrator.getInitialSz(), 0.0);
23067 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
23068 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
23069 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
23070 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
23071 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
23072 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
23073 final var bias1 = calibrator.getBias();
23074 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23075 final var bias2 = new double[3];
23076 calibrator.getBias(bias2);
23077 assertArrayEquals(bias1, bias2, 0.0);
23078 final var b1 = calibrator.getBiasAsMatrix();
23079 assertEquals(b1, ba);
23080 final var b2 = new Matrix(3, 1);
23081 calibrator.getBiasAsMatrix(b2);
23082 assertEquals(b1, b2);
23083 final var ma1 = new Matrix(3, 3);
23084 ma1.setSubmatrix(0, 0, 2, 2,
23085 new double[]{sx, myx, mzx,
23086 mxy, sy, mzy,
23087 mxz, myz, sz});
23088 assertEquals(calibrator.getInitialMa(), ma1);
23089 final var ma2 = new Matrix(3, 3);
23090 calibrator.getInitialMa(ma2);
23091 assertEquals(ma1, ma2);
23092 assertSame(measurements, calibrator.getMeasurements());
23093 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
23094 calibrator.getMeasurementType());
23095 assertFalse(calibrator.isOrderedMeasurementsRequired());
23096 assertFalse(calibrator.isQualityScoresRequired());
23097 assertFalse(calibrator.isCommonAxisUsed());
23098 assertNull(calibrator.getListener());
23099 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
23100 assertFalse(calibrator.isReady());
23101 assertFalse(calibrator.isRunning());
23102 assertNull(calibrator.getEstimatedMa());
23103 assertNull(calibrator.getEstimatedSx());
23104 assertNull(calibrator.getEstimatedSy());
23105 assertNull(calibrator.getEstimatedSz());
23106 assertNull(calibrator.getEstimatedMxy());
23107 assertNull(calibrator.getEstimatedMxz());
23108 assertNull(calibrator.getEstimatedMyx());
23109 assertNull(calibrator.getEstimatedMyz());
23110 assertNull(calibrator.getEstimatedMzx());
23111 assertNull(calibrator.getEstimatedMzy());
23112 assertNull(calibrator.getEstimatedCovariance());
23113 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
23114 assertNotNull(calibrator.getGroundTruthGravityNorm());
23115 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23116 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23117 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
23118 ABSOLUTE_ERROR));
23119 final var g = new Acceleration(0.0, AccelerationUnit.G);
23120 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23121 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23122 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
23123
23124
23125 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23126
23127 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
23128 invalidGravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
23129 }
23130
23131 @Test
23132 void testConstructor198() throws WrongSizeException {
23133 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
23134
23135 final var ba = generateBa();
23136 final var biasX = ba.getElementAtIndex(0);
23137 final var biasY = ba.getElementAtIndex(1);
23138 final var biasZ = ba.getElementAtIndex(2);
23139
23140 final var ma = generateMaCommonAxis();
23141 final var sx = ma.getElementAt(0, 0);
23142 final var sy = ma.getElementAt(1, 1);
23143 final var sz = ma.getElementAt(2, 2);
23144 final var mxy = ma.getElementAt(0, 1);
23145 final var mxz = ma.getElementAt(0, 2);
23146 final var myx = ma.getElementAt(1, 0);
23147 final var myz = ma.getElementAt(1, 2);
23148 final var mzx = ma.getElementAt(2, 0);
23149 final var mzy = ma.getElementAt(2, 1);
23150
23151 final var randomizer = new UniformRandomizer();
23152 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23153 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23154 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23155 final var nedPosition = new NEDPosition(latitude, longitude, height);
23156 final var nedVelocity = new NEDVelocity();
23157 final var ecefPosition = new ECEFPosition();
23158 final var ecefVelocity = new ECEFVelocity();
23159 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23160 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23161 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23162 final var gravityNorm = gravity.getNormAsAcceleration();
23163
23164 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
23165 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
23166
23167
23168 assertEquals(biasX, calibrator.getBiasX(), 0.0);
23169 assertEquals(biasY, calibrator.getBiasY(), 0.0);
23170 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23171 final var bx1 = calibrator.getBiasXAsAcceleration();
23172 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23173 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23174 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23175 calibrator.getBiasXAsAcceleration(bx2);
23176 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23177 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23178 final var by1 = calibrator.getBiasYAsAcceleration();
23179 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23180 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23181 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23182 calibrator.getBiasYAsAcceleration(by2);
23183 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
23184 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
23185 final var bz1 = calibrator.getBiasZAsAcceleration();
23186 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
23187 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
23188 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23189 calibrator.getBiasZAsAcceleration(bz2);
23190 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
23191 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
23192 final var biasTriad1 = calibrator.getBiasAsTriad();
23193 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
23194 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
23195 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
23196 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
23197 final var biasTriad2 = new AccelerationTriad();
23198 calibrator.getBiasAsTriad(biasTriad2);
23199 assertEquals(biasTriad1, biasTriad2);
23200 assertEquals(sx, calibrator.getInitialSx(), 0.0);
23201 assertEquals(sy, calibrator.getInitialSy(), 0.0);
23202 assertEquals(sz, calibrator.getInitialSz(), 0.0);
23203 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
23204 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
23205 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
23206 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
23207 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
23208 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
23209 final var bias1 = calibrator.getBias();
23210 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23211 final var bias2 = new double[3];
23212 calibrator.getBias(bias2);
23213 assertArrayEquals(bias1, bias2, 0.0);
23214 final var b1 = calibrator.getBiasAsMatrix();
23215 assertEquals(b1, ba);
23216 final var b2 = new Matrix(3, 1);
23217 calibrator.getBiasAsMatrix(b2);
23218 assertEquals(b1, b2);
23219 final var ma1 = new Matrix(3, 3);
23220 ma1.setSubmatrix(0, 0, 2, 2,
23221 new double[]{sx, myx, mzx,
23222 mxy, sy, mzy,
23223 mxz, myz, sz});
23224 assertEquals(ma1, calibrator.getInitialMa());
23225 final var ma2 = new Matrix(3, 3);
23226 calibrator.getInitialMa(ma2);
23227 assertEquals(ma1, ma2);
23228 assertSame(measurements, calibrator.getMeasurements());
23229 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
23230 calibrator.getMeasurementType());
23231 assertFalse(calibrator.isOrderedMeasurementsRequired());
23232 assertFalse(calibrator.isQualityScoresRequired());
23233 assertFalse(calibrator.isCommonAxisUsed());
23234 assertSame(this, calibrator.getListener());
23235 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
23236 assertFalse(calibrator.isReady());
23237 assertFalse(calibrator.isRunning());
23238 assertNull(calibrator.getEstimatedMa());
23239 assertNull(calibrator.getEstimatedSx());
23240 assertNull(calibrator.getEstimatedSy());
23241 assertNull(calibrator.getEstimatedSz());
23242 assertNull(calibrator.getEstimatedMxy());
23243 assertNull(calibrator.getEstimatedMxz());
23244 assertNull(calibrator.getEstimatedMyx());
23245 assertNull(calibrator.getEstimatedMyz());
23246 assertNull(calibrator.getEstimatedMzx());
23247 assertNull(calibrator.getEstimatedMzy());
23248 assertNull(calibrator.getEstimatedCovariance());
23249 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
23250 assertNotNull(calibrator.getGroundTruthGravityNorm());
23251 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23252 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23253 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
23254 ABSOLUTE_ERROR));
23255 final var g = new Acceleration(0.0, AccelerationUnit.G);
23256 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23257 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23258 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
23259
23260
23261 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23262
23263 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
23264 invalidGravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
23265 this));
23266 }
23267
23268 @Test
23269 void testConstructor199() throws WrongSizeException {
23270 final var ba = generateBa();
23271 final var biasX = ba.getElementAtIndex(0);
23272 final var biasY = ba.getElementAtIndex(1);
23273 final var biasZ = ba.getElementAtIndex(2);
23274
23275 final var ma = generateMaCommonAxis();
23276 final var sx = ma.getElementAt(0, 0);
23277 final var sy = ma.getElementAt(1, 1);
23278 final var sz = ma.getElementAt(2, 2);
23279 final var mxy = ma.getElementAt(0, 1);
23280 final var mxz = ma.getElementAt(0, 2);
23281 final var myx = ma.getElementAt(1, 0);
23282 final var myz = ma.getElementAt(1, 2);
23283 final var mzx = ma.getElementAt(2, 0);
23284 final var mzy = ma.getElementAt(2, 1);
23285
23286 final var randomizer = new UniformRandomizer();
23287 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23288 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23289 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23290 final var nedPosition = new NEDPosition(latitude, longitude, height);
23291 final var nedVelocity = new NEDVelocity();
23292 final var ecefPosition = new ECEFPosition();
23293 final var ecefVelocity = new ECEFVelocity();
23294 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23295 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23296 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23297 final var gravityNorm = gravity.getNormAsAcceleration();
23298
23299 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
23300 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
23301
23302
23303 assertEquals(biasX, calibrator.getBiasX(), 0.0);
23304 assertEquals(biasY, calibrator.getBiasY(), 0.0);
23305 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23306 final var bx1 = calibrator.getBiasXAsAcceleration();
23307 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23308 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23309 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23310 calibrator.getBiasXAsAcceleration(bx2);
23311 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23312 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23313 final var by1 = calibrator.getBiasYAsAcceleration();
23314 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23315 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23316 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23317 calibrator.getBiasYAsAcceleration(by2);
23318 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
23319 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
23320 final var bz1 = calibrator.getBiasZAsAcceleration();
23321 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
23322 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
23323 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23324 calibrator.getBiasZAsAcceleration(bz2);
23325 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
23326 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
23327 final var biasTriad1 = calibrator.getBiasAsTriad();
23328 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
23329 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
23330 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
23331 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
23332 final var biasTriad2 = new AccelerationTriad();
23333 calibrator.getBiasAsTriad(biasTriad2);
23334 assertEquals(biasTriad1, biasTriad2);
23335 assertEquals(sx, calibrator.getInitialSx(), 0.0);
23336 assertEquals(sy, calibrator.getInitialSy(), 0.0);
23337 assertEquals(sz, calibrator.getInitialSz(), 0.0);
23338 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
23339 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
23340 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
23341 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
23342 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
23343 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
23344 final var bias1 = calibrator.getBias();
23345 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23346 final var bias2 = new double[3];
23347 calibrator.getBias(bias2);
23348 assertArrayEquals(bias1, bias2, 0.0);
23349 final var b1 = calibrator.getBiasAsMatrix();
23350 assertEquals(b1, ba);
23351 final var b2 = new Matrix(3, 1);
23352 calibrator.getBiasAsMatrix(b2);
23353 assertEquals(b1, b2);
23354 final var ma1 = new Matrix(3, 3);
23355 ma1.setSubmatrix(0, 0, 2, 2,
23356 new double[]{sx, myx, mzx,
23357 mxy, sy, mzy,
23358 mxz, myz, sz});
23359 assertEquals(calibrator.getInitialMa(), ma1);
23360 final var ma2 = new Matrix(3, 3);
23361 calibrator.getInitialMa(ma2);
23362 assertEquals(ma1, ma2);
23363 assertNull(calibrator.getMeasurements());
23364 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
23365 calibrator.getMeasurementType());
23366 assertFalse(calibrator.isOrderedMeasurementsRequired());
23367 assertFalse(calibrator.isQualityScoresRequired());
23368 assertTrue(calibrator.isCommonAxisUsed());
23369 assertNull(calibrator.getListener());
23370 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
23371 assertFalse(calibrator.isReady());
23372 assertFalse(calibrator.isRunning());
23373 assertNull(calibrator.getEstimatedMa());
23374 assertNull(calibrator.getEstimatedSx());
23375 assertNull(calibrator.getEstimatedSy());
23376 assertNull(calibrator.getEstimatedSz());
23377 assertNull(calibrator.getEstimatedMxy());
23378 assertNull(calibrator.getEstimatedMxz());
23379 assertNull(calibrator.getEstimatedMyx());
23380 assertNull(calibrator.getEstimatedMyz());
23381 assertNull(calibrator.getEstimatedMzx());
23382 assertNull(calibrator.getEstimatedMzy());
23383 assertNull(calibrator.getEstimatedCovariance());
23384 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
23385 assertNotNull(calibrator.getGroundTruthGravityNorm());
23386 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23387 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23388 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
23389 ABSOLUTE_ERROR));
23390 final var g = new Acceleration(0.0, AccelerationUnit.G);
23391 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23392 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23393 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
23394
23395
23396 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23397
23398 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
23399 invalidGravityNorm, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
23400 }
23401
23402 @Test
23403 void testConstructor200() throws WrongSizeException {
23404 final var ba = generateBa();
23405 final var biasX = ba.getElementAtIndex(0);
23406 final var biasY = ba.getElementAtIndex(1);
23407 final var biasZ = ba.getElementAtIndex(2);
23408
23409 final var ma = generateMaCommonAxis();
23410 final var sx = ma.getElementAt(0, 0);
23411 final var sy = ma.getElementAt(1, 1);
23412 final var sz = ma.getElementAt(2, 2);
23413 final var mxy = ma.getElementAt(0, 1);
23414 final var mxz = ma.getElementAt(0, 2);
23415 final var myx = ma.getElementAt(1, 0);
23416 final var myz = ma.getElementAt(1, 2);
23417 final var mzx = ma.getElementAt(2, 0);
23418 final var mzy = ma.getElementAt(2, 1);
23419
23420 final var randomizer = new UniformRandomizer();
23421 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23422 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23423 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23424 final var nedPosition = new NEDPosition(latitude, longitude, height);
23425 final var nedVelocity = new NEDVelocity();
23426 final var ecefPosition = new ECEFPosition();
23427 final var ecefVelocity = new ECEFVelocity();
23428 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23429 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23430 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23431 final var gravityNorm = gravity.getNormAsAcceleration();
23432
23433 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
23434 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
23435
23436
23437 assertEquals(biasX, calibrator.getBiasX(), 0.0);
23438 assertEquals(biasY, calibrator.getBiasY(), 0.0);
23439 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23440 final var bx1 = calibrator.getBiasXAsAcceleration();
23441 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23442 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23443 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23444 calibrator.getBiasXAsAcceleration(bx2);
23445 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23446 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23447 final var by1 = calibrator.getBiasYAsAcceleration();
23448 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23449 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23450 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23451 calibrator.getBiasYAsAcceleration(by2);
23452 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
23453 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
23454 final var bz1 = calibrator.getBiasZAsAcceleration();
23455 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
23456 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
23457 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23458 calibrator.getBiasZAsAcceleration(bz2);
23459 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
23460 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
23461 final var biasTriad1 = calibrator.getBiasAsTriad();
23462 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
23463 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
23464 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
23465 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
23466 final var biasTriad2 = new AccelerationTriad();
23467 calibrator.getBiasAsTriad(biasTriad2);
23468 assertEquals(biasTriad1, biasTriad2);
23469 assertEquals(sx, calibrator.getInitialSx(), 0.0);
23470 assertEquals(sy, calibrator.getInitialSy(), 0.0);
23471 assertEquals(sz, calibrator.getInitialSz(), 0.0);
23472 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
23473 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
23474 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
23475 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
23476 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
23477 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
23478 final var bias1 = calibrator.getBias();
23479 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23480 final var bias2 = new double[3];
23481 calibrator.getBias(bias2);
23482 assertArrayEquals(bias1, bias2, 0.0);
23483 final var b1 = calibrator.getBiasAsMatrix();
23484 assertEquals(b1, ba);
23485 final var b2 = new Matrix(3, 1);
23486 calibrator.getBiasAsMatrix(b2);
23487 assertEquals(b1, b2);
23488 final var ma1 = new Matrix(3, 3);
23489 ma1.setSubmatrix(0, 0, 2, 2,
23490 new double[]{sx, myx, mzx,
23491 mxy, sy, mzy,
23492 mxz, myz, sz});
23493 assertEquals(ma1, calibrator.getInitialMa());
23494 final var ma2 = new Matrix(3, 3);
23495 calibrator.getInitialMa(ma2);
23496 assertEquals(ma1, ma2);
23497 assertNull(calibrator.getMeasurements());
23498 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
23499 calibrator.getMeasurementType());
23500 assertFalse(calibrator.isOrderedMeasurementsRequired());
23501 assertFalse(calibrator.isQualityScoresRequired());
23502 assertTrue(calibrator.isCommonAxisUsed());
23503 assertSame(this, calibrator.getListener());
23504 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
23505 assertFalse(calibrator.isReady());
23506 assertFalse(calibrator.isRunning());
23507 assertNull(calibrator.getEstimatedMa());
23508 assertNull(calibrator.getEstimatedSx());
23509 assertNull(calibrator.getEstimatedSy());
23510 assertNull(calibrator.getEstimatedSz());
23511 assertNull(calibrator.getEstimatedMxy());
23512 assertNull(calibrator.getEstimatedMxz());
23513 assertNull(calibrator.getEstimatedMyx());
23514 assertNull(calibrator.getEstimatedMyz());
23515 assertNull(calibrator.getEstimatedMzx());
23516 assertNull(calibrator.getEstimatedMzy());
23517 assertNull(calibrator.getEstimatedCovariance());
23518 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
23519 assertNotNull(calibrator.getGroundTruthGravityNorm());
23520 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23521 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23522 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
23523 ABSOLUTE_ERROR));
23524 final var g = new Acceleration(0.0, AccelerationUnit.G);
23525 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23526 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23527 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
23528
23529
23530 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23531
23532 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
23533 invalidGravityNorm, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
23534 }
23535
23536 @Test
23537 void testConstructor201() throws WrongSizeException {
23538 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
23539
23540 final var ba = generateBa();
23541 final var biasX = ba.getElementAtIndex(0);
23542 final var biasY = ba.getElementAtIndex(1);
23543 final var biasZ = ba.getElementAtIndex(2);
23544
23545 final var ma = generateMaCommonAxis();
23546 final var sx = ma.getElementAt(0, 0);
23547 final var sy = ma.getElementAt(1, 1);
23548 final var sz = ma.getElementAt(2, 2);
23549 final var mxy = ma.getElementAt(0, 1);
23550 final var mxz = ma.getElementAt(0, 2);
23551 final var myx = ma.getElementAt(1, 0);
23552 final var myz = ma.getElementAt(1, 2);
23553 final var mzx = ma.getElementAt(2, 0);
23554 final var mzy = ma.getElementAt(2, 1);
23555
23556 final var randomizer = new UniformRandomizer();
23557 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23558 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23559 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23560 final var nedPosition = new NEDPosition(latitude, longitude, height);
23561 final var nedVelocity = new NEDVelocity();
23562 final var ecefPosition = new ECEFPosition();
23563 final var ecefVelocity = new ECEFVelocity();
23564 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23565 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23566 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23567 final var gravityNorm = gravity.getNormAsAcceleration();
23568
23569 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
23570 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
23571
23572
23573 assertEquals(biasX, calibrator.getBiasX(), 0.0);
23574 assertEquals(biasY, calibrator.getBiasY(), 0.0);
23575 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23576 final var bx1 = calibrator.getBiasXAsAcceleration();
23577 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23578 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23579 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23580 calibrator.getBiasXAsAcceleration(bx2);
23581 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23582 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23583 final var by1 = calibrator.getBiasYAsAcceleration();
23584 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23585 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23586 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23587 calibrator.getBiasYAsAcceleration(by2);
23588 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
23589 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
23590 final var bz1 = calibrator.getBiasZAsAcceleration();
23591 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
23592 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
23593 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23594 calibrator.getBiasZAsAcceleration(bz2);
23595 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
23596 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
23597 final var biasTriad1 = calibrator.getBiasAsTriad();
23598 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
23599 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
23600 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
23601 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
23602 final var biasTriad2 = new AccelerationTriad();
23603 calibrator.getBiasAsTriad(biasTriad2);
23604 assertEquals(biasTriad1, biasTriad2);
23605 assertEquals(sx, calibrator.getInitialSx(), 0.0);
23606 assertEquals(sy, calibrator.getInitialSy(), 0.0);
23607 assertEquals(sz, calibrator.getInitialSz(), 0.0);
23608 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
23609 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
23610 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
23611 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
23612 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
23613 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
23614 final var bias1 = calibrator.getBias();
23615 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23616 final var bias2 = new double[3];
23617 calibrator.getBias(bias2);
23618 assertArrayEquals(bias1, bias2, 0.0);
23619 final var b1 = calibrator.getBiasAsMatrix();
23620 assertEquals(b1, ba);
23621 final var b2 = new Matrix(3, 1);
23622 calibrator.getBiasAsMatrix(b2);
23623 assertEquals(b1, b2);
23624 final var ma1 = new Matrix(3, 3);
23625 ma1.setSubmatrix(0, 0, 2, 2,
23626 new double[]{sx, myx, mzx,
23627 mxy, sy, mzy,
23628 mxz, myz, sz});
23629 assertEquals(calibrator.getInitialMa(), ma1);
23630 final var ma2 = new Matrix(3, 3);
23631 calibrator.getInitialMa(ma2);
23632 assertEquals(ma1, ma2);
23633 assertSame(measurements, calibrator.getMeasurements());
23634 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
23635 calibrator.getMeasurementType());
23636 assertFalse(calibrator.isOrderedMeasurementsRequired());
23637 assertFalse(calibrator.isQualityScoresRequired());
23638 assertTrue(calibrator.isCommonAxisUsed());
23639 assertNull(calibrator.getListener());
23640 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
23641 assertFalse(calibrator.isReady());
23642 assertFalse(calibrator.isRunning());
23643 assertNull(calibrator.getEstimatedMa());
23644 assertNull(calibrator.getEstimatedSx());
23645 assertNull(calibrator.getEstimatedSy());
23646 assertNull(calibrator.getEstimatedSz());
23647 assertNull(calibrator.getEstimatedMxy());
23648 assertNull(calibrator.getEstimatedMxz());
23649 assertNull(calibrator.getEstimatedMyx());
23650 assertNull(calibrator.getEstimatedMyz());
23651 assertNull(calibrator.getEstimatedMzx());
23652 assertNull(calibrator.getEstimatedMzy());
23653 assertNull(calibrator.getEstimatedCovariance());
23654 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
23655 assertNotNull(calibrator.getGroundTruthGravityNorm());
23656 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23657 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23658 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
23659 ABSOLUTE_ERROR));
23660 final var g = new Acceleration(0.0, AccelerationUnit.G);
23661 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23662 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23663 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
23664
23665
23666 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23667
23668 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
23669 invalidGravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
23670 }
23671
23672 @Test
23673 void testConstructor202() throws WrongSizeException {
23674 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
23675
23676 final var ba = generateBa();
23677 final var biasX = ba.getElementAtIndex(0);
23678 final var biasY = ba.getElementAtIndex(1);
23679 final var biasZ = ba.getElementAtIndex(2);
23680
23681 final var ma = generateMaCommonAxis();
23682 final var sx = ma.getElementAt(0, 0);
23683 final var sy = ma.getElementAt(1, 1);
23684 final var sz = ma.getElementAt(2, 2);
23685 final var mxy = ma.getElementAt(0, 1);
23686 final var mxz = ma.getElementAt(0, 2);
23687 final var myx = ma.getElementAt(1, 0);
23688 final var myz = ma.getElementAt(1, 2);
23689 final var mzx = ma.getElementAt(2, 0);
23690 final var mzy = ma.getElementAt(2, 1);
23691
23692 final var randomizer = new UniformRandomizer();
23693 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23694 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23695 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23696 final var nedPosition = new NEDPosition(latitude, longitude, height);
23697 final var nedVelocity = new NEDVelocity();
23698 final var ecefPosition = new ECEFPosition();
23699 final var ecefVelocity = new ECEFVelocity();
23700 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23701 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23702 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23703 final var gravityNorm = gravity.getNormAsAcceleration();
23704
23705 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
23706 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
23707
23708
23709 assertEquals(biasX, calibrator.getBiasX(), 0.0);
23710 assertEquals(biasY, calibrator.getBiasY(), 0.0);
23711 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23712 final var bx1 = calibrator.getBiasXAsAcceleration();
23713 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23714 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23715 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23716 calibrator.getBiasXAsAcceleration(bx2);
23717 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23718 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23719 final var by1 = calibrator.getBiasYAsAcceleration();
23720 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23721 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23722 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23723 calibrator.getBiasYAsAcceleration(by2);
23724 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
23725 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
23726 final var bz1 = calibrator.getBiasZAsAcceleration();
23727 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
23728 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
23729 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23730 calibrator.getBiasZAsAcceleration(bz2);
23731 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
23732 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
23733 final var biasTriad1 = calibrator.getBiasAsTriad();
23734 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
23735 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
23736 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
23737 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
23738 final var biasTriad2 = new AccelerationTriad();
23739 calibrator.getBiasAsTriad(biasTriad2);
23740 assertEquals(biasTriad1, biasTriad2);
23741 assertEquals(sx, calibrator.getInitialSx(), 0.0);
23742 assertEquals(sy, calibrator.getInitialSy(), 0.0);
23743 assertEquals(sz, calibrator.getInitialSz(), 0.0);
23744 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
23745 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
23746 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
23747 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
23748 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
23749 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
23750 final var bias1 = calibrator.getBias();
23751 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23752 final var bias2 = new double[3];
23753 calibrator.getBias(bias2);
23754 assertArrayEquals(bias1, bias2, 0.0);
23755 final var b1 = calibrator.getBiasAsMatrix();
23756 assertEquals(b1, ba);
23757 final var b2 = new Matrix(3, 1);
23758 calibrator.getBiasAsMatrix(b2);
23759 assertEquals(b1, b2);
23760 final var ma1 = new Matrix(3, 3);
23761 ma1.setSubmatrix(0, 0, 2, 2,
23762 new double[]{sx, myx, mzx,
23763 mxy, sy, mzy,
23764 mxz, myz, sz});
23765 assertEquals(calibrator.getInitialMa(), ma1);
23766 final var ma2 = new Matrix(3, 3);
23767 calibrator.getInitialMa(ma2);
23768 assertEquals(ma1, ma2);
23769 assertSame(measurements, calibrator.getMeasurements());
23770 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
23771 calibrator.getMeasurementType());
23772 assertFalse(calibrator.isOrderedMeasurementsRequired());
23773 assertFalse(calibrator.isQualityScoresRequired());
23774 assertTrue(calibrator.isCommonAxisUsed());
23775 assertSame(this, calibrator.getListener());
23776 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
23777 assertFalse(calibrator.isReady());
23778 assertFalse(calibrator.isRunning());
23779 assertNull(calibrator.getEstimatedMa());
23780 assertNull(calibrator.getEstimatedSx());
23781 assertNull(calibrator.getEstimatedSy());
23782 assertNull(calibrator.getEstimatedSz());
23783 assertNull(calibrator.getEstimatedMxy());
23784 assertNull(calibrator.getEstimatedMxz());
23785 assertNull(calibrator.getEstimatedMyx());
23786 assertNull(calibrator.getEstimatedMyz());
23787 assertNull(calibrator.getEstimatedMzx());
23788 assertNull(calibrator.getEstimatedMzy());
23789 assertNull(calibrator.getEstimatedCovariance());
23790 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
23791 assertNotNull(calibrator.getGroundTruthGravityNorm());
23792 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23793 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23794 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
23795 ABSOLUTE_ERROR));
23796 final var g = new Acceleration(0.0, AccelerationUnit.G);
23797 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23798 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23799 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
23800
23801
23802 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23803
23804 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
23805 invalidGravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23806 myx, myz, mzx, mzy, this));
23807 }
23808
23809 @Test
23810 void testConstructor203() throws WrongSizeException {
23811 final var ba = generateBa();
23812 final var biasX = ba.getElementAtIndex(0);
23813 final var biasY = ba.getElementAtIndex(1);
23814 final var biasZ = ba.getElementAtIndex(2);
23815
23816 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
23817 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
23818 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
23819
23820 final var ma = generateMaCommonAxis();
23821 final var sx = ma.getElementAt(0, 0);
23822 final var sy = ma.getElementAt(1, 1);
23823 final var sz = ma.getElementAt(2, 2);
23824 final var mxy = ma.getElementAt(0, 1);
23825 final var mxz = ma.getElementAt(0, 2);
23826 final var myx = ma.getElementAt(1, 0);
23827 final var myz = ma.getElementAt(1, 2);
23828 final var mzx = ma.getElementAt(2, 0);
23829 final var mzy = ma.getElementAt(2, 1);
23830
23831 final var randomizer = new UniformRandomizer();
23832 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23833 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23834 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23835 final var nedPosition = new NEDPosition(latitude, longitude, height);
23836 final var nedVelocity = new NEDVelocity();
23837 final var ecefPosition = new ECEFPosition();
23838 final var ecefVelocity = new ECEFVelocity();
23839 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23840 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23841 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23842 final var gravityNorm = gravity.getNormAsAcceleration();
23843
23844 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz,
23845 mxy, mxz, myx, myz, mzx, mzy);
23846
23847
23848 assertEquals(biasX, calibrator.getBiasX(), 0.0);
23849 assertEquals(biasY, calibrator.getBiasY(), 0.0);
23850 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23851 final var bx1 = calibrator.getBiasXAsAcceleration();
23852 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23853 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23854 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23855 calibrator.getBiasXAsAcceleration(bx2);
23856 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23857 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23858 final var by1 = calibrator.getBiasYAsAcceleration();
23859 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23860 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23861 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23862 calibrator.getBiasYAsAcceleration(by2);
23863 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
23864 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
23865 final var bz1 = calibrator.getBiasZAsAcceleration();
23866 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
23867 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
23868 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23869 calibrator.getBiasZAsAcceleration(bz2);
23870 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
23871 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
23872 final var biasTriad1 = calibrator.getBiasAsTriad();
23873 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
23874 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
23875 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
23876 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
23877 final var biasTriad2 = new AccelerationTriad();
23878 calibrator.getBiasAsTriad(biasTriad2);
23879 assertEquals(biasTriad1, biasTriad2);
23880 assertEquals(sx, calibrator.getInitialSx(), 0.0);
23881 assertEquals(sy, calibrator.getInitialSy(), 0.0);
23882 assertEquals(sz, calibrator.getInitialSz(), 0.0);
23883 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
23884 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
23885 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
23886 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
23887 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
23888 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
23889 final var bias1 = calibrator.getBias();
23890 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23891 final var bias2 = new double[3];
23892 calibrator.getBias(bias2);
23893 assertArrayEquals(bias1, bias2, 0.0);
23894 final var b1 = calibrator.getBiasAsMatrix();
23895 assertEquals(b1, ba);
23896 final var b2 = new Matrix(3, 1);
23897 calibrator.getBiasAsMatrix(b2);
23898 assertEquals(b1, b2);
23899 final var ma1 = new Matrix(3, 3);
23900 ma1.setSubmatrix(0, 0, 2, 2,
23901 new double[]{sx, myx, mzx,
23902 mxy, sy, mzy,
23903 mxz, myz, sz});
23904 assertEquals(ma1, calibrator.getInitialMa());
23905 final var ma2 = new Matrix(3, 3);
23906 calibrator.getInitialMa(ma2);
23907 assertEquals(ma1, ma2);
23908 assertNull(calibrator.getMeasurements());
23909 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
23910 calibrator.getMeasurementType());
23911 assertFalse(calibrator.isOrderedMeasurementsRequired());
23912 assertFalse(calibrator.isQualityScoresRequired());
23913 assertFalse(calibrator.isCommonAxisUsed());
23914 assertNull(calibrator.getListener());
23915 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
23916 assertFalse(calibrator.isReady());
23917 assertFalse(calibrator.isRunning());
23918 assertNull(calibrator.getEstimatedMa());
23919 assertNull(calibrator.getEstimatedSx());
23920 assertNull(calibrator.getEstimatedSy());
23921 assertNull(calibrator.getEstimatedSz());
23922 assertNull(calibrator.getEstimatedMxy());
23923 assertNull(calibrator.getEstimatedMxz());
23924 assertNull(calibrator.getEstimatedMyx());
23925 assertNull(calibrator.getEstimatedMyz());
23926 assertNull(calibrator.getEstimatedMzx());
23927 assertNull(calibrator.getEstimatedMzy());
23928 assertNull(calibrator.getEstimatedCovariance());
23929 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
23930 assertNotNull(calibrator.getGroundTruthGravityNorm());
23931 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23932 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23933 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
23934 ABSOLUTE_ERROR));
23935 final var g = new Acceleration(0.0, AccelerationUnit.G);
23936 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23937 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23938 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
23939
23940
23941 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23942
23943 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
23944 invalidGravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
23945 }
23946
23947 @Test
23948 void testConstructor204() throws WrongSizeException {
23949 final var ba = generateBa();
23950 final var biasX = ba.getElementAtIndex(0);
23951 final var biasY = ba.getElementAtIndex(1);
23952 final var biasZ = ba.getElementAtIndex(2);
23953
23954 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
23955 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
23956 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
23957
23958 final var ma = generateMaCommonAxis();
23959 final var sx = ma.getElementAt(0, 0);
23960 final var sy = ma.getElementAt(1, 1);
23961 final var sz = ma.getElementAt(2, 2);
23962 final var mxy = ma.getElementAt(0, 1);
23963 final var mxz = ma.getElementAt(0, 2);
23964 final var myx = ma.getElementAt(1, 0);
23965 final var myz = ma.getElementAt(1, 2);
23966 final var mzx = ma.getElementAt(2, 0);
23967 final var mzy = ma.getElementAt(2, 1);
23968
23969 final var randomizer = new UniformRandomizer();
23970 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23971 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23972 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23973 final var nedPosition = new NEDPosition(latitude, longitude, height);
23974 final var nedVelocity = new NEDVelocity();
23975 final var ecefPosition = new ECEFPosition();
23976 final var ecefVelocity = new ECEFVelocity();
23977 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23978 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23979 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23980 final var gravityNorm = gravity.getNormAsAcceleration();
23981
23982 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz,
23983 mxy, mxz, myx, myz, mzx, mzy, this);
23984
23985
23986 assertEquals(biasX, calibrator.getBiasX(), 0.0);
23987 assertEquals(biasY, calibrator.getBiasY(), 0.0);
23988 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23989 final var bx1 = calibrator.getBiasXAsAcceleration();
23990 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23991 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23992 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23993 calibrator.getBiasXAsAcceleration(bx2);
23994 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23995 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23996 final var by1 = calibrator.getBiasYAsAcceleration();
23997 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23998 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23999 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24000 calibrator.getBiasYAsAcceleration(by2);
24001 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24002 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24003 final var bz1 = calibrator.getBiasZAsAcceleration();
24004 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
24005 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24006 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24007 calibrator.getBiasZAsAcceleration(bz2);
24008 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24009 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24010 final var biasTriad1 = calibrator.getBiasAsTriad();
24011 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24012 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24013 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24014 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24015 final var biasTriad2 = new AccelerationTriad();
24016 calibrator.getBiasAsTriad(biasTriad2);
24017 assertEquals(biasTriad1, biasTriad2);
24018 assertEquals(sx, calibrator.getInitialSx(), 0.0);
24019 assertEquals(sy, calibrator.getInitialSy(), 0.0);
24020 assertEquals(sz, calibrator.getInitialSz(), 0.0);
24021 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
24022 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
24023 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
24024 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
24025 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
24026 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
24027 final var bias1 = calibrator.getBias();
24028 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24029 final var bias2 = new double[3];
24030 calibrator.getBias(bias2);
24031 assertArrayEquals(bias1, bias2, 0.0);
24032 final var b1 = calibrator.getBiasAsMatrix();
24033 assertEquals(b1, ba);
24034 final var b2 = new Matrix(3, 1);
24035 calibrator.getBiasAsMatrix(b2);
24036 assertEquals(b1, b2);
24037 final var ma1 = new Matrix(3, 3);
24038 ma1.setSubmatrix(0, 0, 2, 2,
24039 new double[]{sx, myx, mzx,
24040 mxy, sy, mzy,
24041 mxz, myz, sz});
24042 assertEquals(ma1, calibrator.getInitialMa());
24043 final var ma2 = new Matrix(3, 3);
24044 calibrator.getInitialMa(ma2);
24045 assertEquals(ma1, ma2);
24046 assertNull(calibrator.getMeasurements());
24047 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
24048 calibrator.getMeasurementType());
24049 assertFalse(calibrator.isOrderedMeasurementsRequired());
24050 assertFalse(calibrator.isQualityScoresRequired());
24051 assertFalse(calibrator.isCommonAxisUsed());
24052 assertSame(this, calibrator.getListener());
24053 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
24054 assertFalse(calibrator.isReady());
24055 assertFalse(calibrator.isRunning());
24056 assertNull(calibrator.getEstimatedMa());
24057 assertNull(calibrator.getEstimatedSx());
24058 assertNull(calibrator.getEstimatedSy());
24059 assertNull(calibrator.getEstimatedSz());
24060 assertNull(calibrator.getEstimatedMxy());
24061 assertNull(calibrator.getEstimatedMxz());
24062 assertNull(calibrator.getEstimatedMyx());
24063 assertNull(calibrator.getEstimatedMyz());
24064 assertNull(calibrator.getEstimatedMzx());
24065 assertNull(calibrator.getEstimatedMzy());
24066 assertNull(calibrator.getEstimatedCovariance());
24067 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
24068 assertNotNull(calibrator.getGroundTruthGravityNorm());
24069 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24070 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24071 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
24072 ABSOLUTE_ERROR));
24073 final var g = new Acceleration(0.0, AccelerationUnit.G);
24074 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24075 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24076 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
24077
24078
24079 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24080
24081 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
24082 invalidGravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
24083 }
24084
24085 @Test
24086 void testConstructor205() throws WrongSizeException {
24087 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
24088
24089 final var ba = generateBa();
24090 final var biasX = ba.getElementAtIndex(0);
24091 final var biasY = ba.getElementAtIndex(1);
24092 final var biasZ = ba.getElementAtIndex(2);
24093
24094 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24095 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24096 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24097
24098 final var ma = generateMaCommonAxis();
24099 final var sx = ma.getElementAt(0, 0);
24100 final var sy = ma.getElementAt(1, 1);
24101 final var sz = ma.getElementAt(2, 2);
24102 final var mxy = ma.getElementAt(0, 1);
24103 final var mxz = ma.getElementAt(0, 2);
24104 final var myx = ma.getElementAt(1, 0);
24105 final var myz = ma.getElementAt(1, 2);
24106 final var mzx = ma.getElementAt(2, 0);
24107 final var mzy = ma.getElementAt(2, 1);
24108
24109 final var randomizer = new UniformRandomizer();
24110 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24111 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24112 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24113 final var nedPosition = new NEDPosition(latitude, longitude, height);
24114 final var nedVelocity = new NEDVelocity();
24115 final var ecefPosition = new ECEFPosition();
24116 final var ecefVelocity = new ECEFVelocity();
24117 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
24118 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24119 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24120 final var gravityNorm = gravity.getNormAsAcceleration();
24121
24122 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
24123 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
24124
24125
24126 assertEquals(biasX, calibrator.getBiasX(), 0.0);
24127 assertEquals(biasY, calibrator.getBiasY(), 0.0);
24128 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
24129 final var bx1 = calibrator.getBiasXAsAcceleration();
24130 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
24131 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
24132 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24133 calibrator.getBiasXAsAcceleration(bx2);
24134 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
24135 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
24136 final var by1 = calibrator.getBiasYAsAcceleration();
24137 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
24138 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
24139 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24140 calibrator.getBiasYAsAcceleration(by2);
24141 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24142 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24143 final var bz1 = calibrator.getBiasZAsAcceleration();
24144 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
24145 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24146 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24147 calibrator.getBiasZAsAcceleration(bz2);
24148 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24149 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24150 final var biasTriad1 = calibrator.getBiasAsTriad();
24151 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24152 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24153 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24154 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24155 final var biasTriad2 = new AccelerationTriad();
24156 calibrator.getBiasAsTriad(biasTriad2);
24157 assertEquals(biasTriad1, biasTriad2);
24158 assertEquals(sx, calibrator.getInitialSx(), 0.0);
24159 assertEquals(sy, calibrator.getInitialSy(), 0.0);
24160 assertEquals(sz, calibrator.getInitialSz(), 0.0);
24161 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
24162 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
24163 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
24164 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
24165 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
24166 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
24167 final var bias1 = calibrator.getBias();
24168 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24169 final var bias2 = new double[3];
24170 calibrator.getBias(bias2);
24171 assertArrayEquals(bias1, bias2, 0.0);
24172 final var b1 = calibrator.getBiasAsMatrix();
24173 assertEquals(b1, ba);
24174 final var b2 = new Matrix(3, 1);
24175 calibrator.getBiasAsMatrix(b2);
24176 assertEquals(b1, b2);
24177 final var ma1 = new Matrix(3, 3);
24178 ma1.setSubmatrix(0, 0, 2, 2,
24179 new double[]{sx, myx, mzx,
24180 mxy, sy, mzy,
24181 mxz, myz, sz});
24182 assertEquals(calibrator.getInitialMa(), ma1);
24183 final var ma2 = new Matrix(3, 3);
24184 calibrator.getInitialMa(ma2);
24185 assertEquals(ma1, ma2);
24186 assertSame(measurements, calibrator.getMeasurements());
24187 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
24188 calibrator.getMeasurementType());
24189 assertFalse(calibrator.isOrderedMeasurementsRequired());
24190 assertFalse(calibrator.isQualityScoresRequired());
24191 assertFalse(calibrator.isCommonAxisUsed());
24192 assertNull(calibrator.getListener());
24193 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
24194 assertFalse(calibrator.isReady());
24195 assertFalse(calibrator.isRunning());
24196 assertNull(calibrator.getEstimatedMa());
24197 assertNull(calibrator.getEstimatedSx());
24198 assertNull(calibrator.getEstimatedSy());
24199 assertNull(calibrator.getEstimatedSz());
24200 assertNull(calibrator.getEstimatedMxy());
24201 assertNull(calibrator.getEstimatedMxz());
24202 assertNull(calibrator.getEstimatedMyx());
24203 assertNull(calibrator.getEstimatedMyz());
24204 assertNull(calibrator.getEstimatedMzx());
24205 assertNull(calibrator.getEstimatedMzy());
24206 assertNull(calibrator.getEstimatedCovariance());
24207 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
24208 assertNotNull(calibrator.getGroundTruthGravityNorm());
24209 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24210 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24211 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
24212 ABSOLUTE_ERROR));
24213 final var g = new Acceleration(0.0, AccelerationUnit.G);
24214 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24215 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24216 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
24217
24218
24219 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24220
24221 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
24222 invalidGravityNorm, measurements, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
24223 }
24224
24225 @Test
24226 void testConstructor206() throws WrongSizeException {
24227 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
24228
24229 final var ba = generateBa();
24230 final var biasX = ba.getElementAtIndex(0);
24231 final var biasY = ba.getElementAtIndex(1);
24232 final var biasZ = ba.getElementAtIndex(2);
24233
24234 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24235 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24236 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24237
24238 final var ma = generateMaCommonAxis();
24239 final var sx = ma.getElementAt(0, 0);
24240 final var sy = ma.getElementAt(1, 1);
24241 final var sz = ma.getElementAt(2, 2);
24242 final var mxy = ma.getElementAt(0, 1);
24243 final var mxz = ma.getElementAt(0, 2);
24244 final var myx = ma.getElementAt(1, 0);
24245 final var myz = ma.getElementAt(1, 2);
24246 final var mzx = ma.getElementAt(2, 0);
24247 final var mzy = ma.getElementAt(2, 1);
24248
24249 final var randomizer = new UniformRandomizer();
24250 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24251 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24252 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24253 final var nedPosition = new NEDPosition(latitude, longitude, height);
24254 final var nedVelocity = new NEDVelocity();
24255 final var ecefPosition = new ECEFPosition();
24256 final var ecefVelocity = new ECEFVelocity();
24257 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
24258 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24259 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24260 final var gravityNorm = gravity.getNormAsAcceleration();
24261
24262 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
24263 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
24264
24265
24266 assertEquals(biasX, calibrator.getBiasX(), 0.0);
24267 assertEquals(biasY, calibrator.getBiasY(), 0.0);
24268 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
24269 final var bx1 = calibrator.getBiasXAsAcceleration();
24270 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
24271 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
24272 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24273 calibrator.getBiasXAsAcceleration(bx2);
24274 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
24275 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
24276 final var by1 = calibrator.getBiasYAsAcceleration();
24277 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
24278 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
24279 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24280 calibrator.getBiasYAsAcceleration(by2);
24281 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24282 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24283 final var bz1 = calibrator.getBiasZAsAcceleration();
24284 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
24285 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24286 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24287 calibrator.getBiasZAsAcceleration(bz2);
24288 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24289 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24290 final var biasTriad1 = calibrator.getBiasAsTriad();
24291 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24292 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24293 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24294 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24295 final var biasTriad2 = new AccelerationTriad();
24296 calibrator.getBiasAsTriad(biasTriad2);
24297 assertEquals(biasTriad1, biasTriad2);
24298 assertEquals(sx, calibrator.getInitialSx(), 0.0);
24299 assertEquals(sy, calibrator.getInitialSy(), 0.0);
24300 assertEquals(sz, calibrator.getInitialSz(), 0.0);
24301 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
24302 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
24303 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
24304 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
24305 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
24306 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
24307 final var bias1 = calibrator.getBias();
24308 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24309 final var bias2 = new double[3];
24310 calibrator.getBias(bias2);
24311 assertArrayEquals(bias1, bias2, 0.0);
24312 final var b1 = calibrator.getBiasAsMatrix();
24313 assertEquals(b1, ba);
24314 final var b2 = new Matrix(3, 1);
24315 calibrator.getBiasAsMatrix(b2);
24316 assertEquals(b1, b2);
24317 final var ma1 = new Matrix(3, 3);
24318 ma1.setSubmatrix(0, 0, 2, 2,
24319 new double[]{sx, myx, mzx,
24320 mxy, sy, mzy,
24321 mxz, myz, sz});
24322 assertEquals(ma1, calibrator.getInitialMa());
24323 final var ma2 = new Matrix(3, 3);
24324 calibrator.getInitialMa(ma2);
24325 assertEquals(ma1, ma2);
24326 assertSame(measurements, calibrator.getMeasurements());
24327 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
24328 calibrator.getMeasurementType());
24329 assertFalse(calibrator.isOrderedMeasurementsRequired());
24330 assertFalse(calibrator.isQualityScoresRequired());
24331 assertFalse(calibrator.isCommonAxisUsed());
24332 assertSame(this, calibrator.getListener());
24333 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
24334 assertFalse(calibrator.isReady());
24335 assertFalse(calibrator.isRunning());
24336 assertNull(calibrator.getEstimatedMa());
24337 assertNull(calibrator.getEstimatedSx());
24338 assertNull(calibrator.getEstimatedSy());
24339 assertNull(calibrator.getEstimatedSz());
24340 assertNull(calibrator.getEstimatedMxy());
24341 assertNull(calibrator.getEstimatedMxz());
24342 assertNull(calibrator.getEstimatedMyx());
24343 assertNull(calibrator.getEstimatedMyz());
24344 assertNull(calibrator.getEstimatedMzx());
24345 assertNull(calibrator.getEstimatedMzy());
24346 assertNull(calibrator.getEstimatedCovariance());
24347 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
24348 assertNotNull(calibrator.getGroundTruthGravityNorm());
24349 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24350 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24351 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
24352 ABSOLUTE_ERROR));
24353 final var g = new Acceleration(0.0, AccelerationUnit.G);
24354 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24355 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24356 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
24357
24358
24359 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24360
24361 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
24362 invalidGravityNorm, measurements, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
24363 }
24364
24365 @Test
24366 void testConstructor207() throws WrongSizeException {
24367 final var ba = generateBa();
24368 final var biasX = ba.getElementAtIndex(0);
24369 final var biasY = ba.getElementAtIndex(1);
24370 final var biasZ = ba.getElementAtIndex(2);
24371
24372 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24373 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24374 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24375
24376 final var ma = generateMaCommonAxis();
24377 final var sx = ma.getElementAt(0, 0);
24378 final var sy = ma.getElementAt(1, 1);
24379 final var sz = ma.getElementAt(2, 2);
24380 final var mxy = ma.getElementAt(0, 1);
24381 final var mxz = ma.getElementAt(0, 2);
24382 final var myx = ma.getElementAt(1, 0);
24383 final var myz = ma.getElementAt(1, 2);
24384 final var mzx = ma.getElementAt(2, 0);
24385 final var mzy = ma.getElementAt(2, 1);
24386
24387 final var randomizer = new UniformRandomizer();
24388 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24389 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24390 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24391 final var nedPosition = new NEDPosition(latitude, longitude, height);
24392 final var nedVelocity = new NEDVelocity();
24393 final var ecefPosition = new ECEFPosition();
24394 final var ecefVelocity = new ECEFVelocity();
24395 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
24396 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24397 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24398 final var gravityNorm = gravity.getNormAsAcceleration();
24399
24400 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
24401 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
24402
24403
24404 assertEquals(biasX, calibrator.getBiasX(), 0.0);
24405 assertEquals(biasY, calibrator.getBiasY(), 0.0);
24406 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
24407 final var bx1 = calibrator.getBiasXAsAcceleration();
24408 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
24409 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
24410 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24411 calibrator.getBiasXAsAcceleration(bx2);
24412 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
24413 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
24414 final var by1 = calibrator.getBiasYAsAcceleration();
24415 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
24416 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
24417 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24418 calibrator.getBiasYAsAcceleration(by2);
24419 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24420 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24421 final var bz1 = calibrator.getBiasZAsAcceleration();
24422 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
24423 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24424 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24425 calibrator.getBiasZAsAcceleration(bz2);
24426 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24427 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24428 final var biasTriad1 = calibrator.getBiasAsTriad();
24429 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24430 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24431 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24432 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24433 final var biasTriad2 = new AccelerationTriad();
24434 calibrator.getBiasAsTriad(biasTriad2);
24435 assertEquals(biasTriad1, biasTriad2);
24436 assertEquals(sx, calibrator.getInitialSx(), 0.0);
24437 assertEquals(sy, calibrator.getInitialSy(), 0.0);
24438 assertEquals(sz, calibrator.getInitialSz(), 0.0);
24439 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
24440 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
24441 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
24442 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
24443 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
24444 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
24445 final var bias1 = calibrator.getBias();
24446 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24447 final var bias2 = new double[3];
24448 calibrator.getBias(bias2);
24449 assertArrayEquals(bias1, bias2, 0.0);
24450 final var b1 = calibrator.getBiasAsMatrix();
24451 assertEquals(b1, ba);
24452 final var b2 = new Matrix(3, 1);
24453 calibrator.getBiasAsMatrix(b2);
24454 assertEquals(b1, b2);
24455 final var ma1 = new Matrix(3, 3);
24456 ma1.setSubmatrix(0, 0, 2, 2,
24457 new double[]{sx, myx, mzx,
24458 mxy, sy, mzy,
24459 mxz, myz, sz});
24460 assertEquals(ma1, calibrator.getInitialMa());
24461 final var ma2 = new Matrix(3, 3);
24462 calibrator.getInitialMa(ma2);
24463 assertEquals(ma1, ma2);
24464 assertNull(calibrator.getMeasurements());
24465 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
24466 calibrator.getMeasurementType());
24467 assertFalse(calibrator.isOrderedMeasurementsRequired());
24468 assertFalse(calibrator.isQualityScoresRequired());
24469 assertTrue(calibrator.isCommonAxisUsed());
24470 assertNull(calibrator.getListener());
24471 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
24472 assertFalse(calibrator.isReady());
24473 assertFalse(calibrator.isRunning());
24474 assertNull(calibrator.getEstimatedMa());
24475 assertNull(calibrator.getEstimatedSx());
24476 assertNull(calibrator.getEstimatedSy());
24477 assertNull(calibrator.getEstimatedSz());
24478 assertNull(calibrator.getEstimatedMxy());
24479 assertNull(calibrator.getEstimatedMxz());
24480 assertNull(calibrator.getEstimatedMyx());
24481 assertNull(calibrator.getEstimatedMyz());
24482 assertNull(calibrator.getEstimatedMzx());
24483 assertNull(calibrator.getEstimatedMzy());
24484 assertNull(calibrator.getEstimatedCovariance());
24485 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
24486 assertNotNull(calibrator.getGroundTruthGravityNorm());
24487 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24488 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24489 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
24490 ABSOLUTE_ERROR));
24491 final var g = new Acceleration(0.0, AccelerationUnit.G);
24492 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24493 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24494 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
24495
24496
24497 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24498
24499 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
24500 invalidGravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
24501 }
24502
24503 @Test
24504 void testConstructor208() throws WrongSizeException {
24505 final var ba = generateBa();
24506 final var biasX = ba.getElementAtIndex(0);
24507 final var biasY = ba.getElementAtIndex(1);
24508 final var biasZ = ba.getElementAtIndex(2);
24509
24510 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24511 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24512 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24513
24514 final var ma = generateMaCommonAxis();
24515 final var sx = ma.getElementAt(0, 0);
24516 final var sy = ma.getElementAt(1, 1);
24517 final var sz = ma.getElementAt(2, 2);
24518 final var mxy = ma.getElementAt(0, 1);
24519 final var mxz = ma.getElementAt(0, 2);
24520 final var myx = ma.getElementAt(1, 0);
24521 final var myz = ma.getElementAt(1, 2);
24522 final var mzx = ma.getElementAt(2, 0);
24523 final var mzy = ma.getElementAt(2, 1);
24524
24525 final var randomizer = new UniformRandomizer();
24526 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24527 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24528 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24529 final var nedPosition = new NEDPosition(latitude, longitude, height);
24530 final var nedVelocity = new NEDVelocity();
24531 final var ecefPosition = new ECEFPosition();
24532 final var ecefVelocity = new ECEFVelocity();
24533 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
24534 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24535 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24536 final var gravityNorm = gravity.getNormAsAcceleration();
24537
24538 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
24539 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
24540
24541
24542 assertEquals(biasX, calibrator.getBiasX(), 0.0);
24543 assertEquals(biasY, calibrator.getBiasY(), 0.0);
24544 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
24545 final var bx1 = calibrator.getBiasXAsAcceleration();
24546 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
24547 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
24548 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24549 calibrator.getBiasXAsAcceleration(bx2);
24550 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
24551 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
24552 final var by1 = calibrator.getBiasYAsAcceleration();
24553 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
24554 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
24555 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24556 calibrator.getBiasYAsAcceleration(by2);
24557 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24558 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24559 final var bz1 = calibrator.getBiasZAsAcceleration();
24560 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
24561 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24562 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24563 calibrator.getBiasZAsAcceleration(bz2);
24564 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24565 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24566 final var biasTriad1 = calibrator.getBiasAsTriad();
24567 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24568 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24569 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24570 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24571 final var biasTriad2 = new AccelerationTriad();
24572 calibrator.getBiasAsTriad(biasTriad2);
24573 assertEquals(biasTriad1, biasTriad2);
24574 assertEquals(sx, calibrator.getInitialSx(), 0.0);
24575 assertEquals(sy, calibrator.getInitialSy(), 0.0);
24576 assertEquals(sz, calibrator.getInitialSz(), 0.0);
24577 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
24578 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
24579 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
24580 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
24581 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
24582 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
24583 final var bias1 = calibrator.getBias();
24584 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24585 final var bias2 = new double[3];
24586 calibrator.getBias(bias2);
24587 assertArrayEquals(bias1, bias2, 0.0);
24588 final var b1 = calibrator.getBiasAsMatrix();
24589 assertEquals(b1, ba);
24590 final var b2 = new Matrix(3, 1);
24591 calibrator.getBiasAsMatrix(b2);
24592 assertEquals(b1, b2);
24593 final var ma1 = new Matrix(3, 3);
24594 ma1.setSubmatrix(0, 0, 2, 2,
24595 new double[]{sx, myx, mzx,
24596 mxy, sy, mzy,
24597 mxz, myz, sz});
24598 assertEquals(calibrator.getInitialMa(), ma1);
24599 final var ma2 = new Matrix(3, 3);
24600 calibrator.getInitialMa(ma2);
24601 assertEquals(ma1, ma2);
24602 assertNull(calibrator.getMeasurements());
24603 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
24604 calibrator.getMeasurementType());
24605 assertFalse(calibrator.isOrderedMeasurementsRequired());
24606 assertFalse(calibrator.isQualityScoresRequired());
24607 assertTrue(calibrator.isCommonAxisUsed());
24608 assertSame(this, calibrator.getListener());
24609 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
24610 assertFalse(calibrator.isReady());
24611 assertFalse(calibrator.isRunning());
24612 assertNull(calibrator.getEstimatedMa());
24613 assertNull(calibrator.getEstimatedSx());
24614 assertNull(calibrator.getEstimatedSy());
24615 assertNull(calibrator.getEstimatedSz());
24616 assertNull(calibrator.getEstimatedMxy());
24617 assertNull(calibrator.getEstimatedMxz());
24618 assertNull(calibrator.getEstimatedMyx());
24619 assertNull(calibrator.getEstimatedMyz());
24620 assertNull(calibrator.getEstimatedMzx());
24621 assertNull(calibrator.getEstimatedMzy());
24622 assertNull(calibrator.getEstimatedCovariance());
24623 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
24624 assertNotNull(calibrator.getGroundTruthGravityNorm());
24625 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24626 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24627 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
24628 ABSOLUTE_ERROR));
24629 final var g = new Acceleration(0.0, AccelerationUnit.G);
24630 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24631 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24632 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
24633
24634
24635 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24636
24637 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
24638 invalidGravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
24639 this));
24640 }
24641
24642 @Test
24643 void testConstructor209() throws WrongSizeException {
24644 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
24645
24646 final var ba = generateBa();
24647 final var biasX = ba.getElementAtIndex(0);
24648 final var biasY = ba.getElementAtIndex(1);
24649 final var biasZ = ba.getElementAtIndex(2);
24650
24651 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24652 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24653 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24654
24655 final var ma = generateMaCommonAxis();
24656 final var sx = ma.getElementAt(0, 0);
24657 final var sy = ma.getElementAt(1, 1);
24658 final var sz = ma.getElementAt(2, 2);
24659 final var mxy = ma.getElementAt(0, 1);
24660 final var mxz = ma.getElementAt(0, 2);
24661 final var myx = ma.getElementAt(1, 0);
24662 final var myz = ma.getElementAt(1, 2);
24663 final var mzx = ma.getElementAt(2, 0);
24664 final var mzy = ma.getElementAt(2, 1);
24665
24666 final var randomizer = new UniformRandomizer();
24667 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24668 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24669 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24670 final var nedPosition = new NEDPosition(latitude, longitude, height);
24671 final var nedVelocity = new NEDVelocity();
24672 final var ecefPosition = new ECEFPosition();
24673 final var ecefVelocity = new ECEFVelocity();
24674 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
24675 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24676 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24677 final var gravityNorm = gravity.getNormAsAcceleration();
24678
24679 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
24680 true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
24681
24682
24683 assertEquals(biasX, calibrator.getBiasX(), 0.0);
24684 assertEquals(biasY, calibrator.getBiasY(), 0.0);
24685 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
24686 final var bx1 = calibrator.getBiasXAsAcceleration();
24687 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
24688 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
24689 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24690 calibrator.getBiasXAsAcceleration(bx2);
24691 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
24692 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
24693 final var by1 = calibrator.getBiasYAsAcceleration();
24694 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
24695 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
24696 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24697 calibrator.getBiasYAsAcceleration(by2);
24698 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24699 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24700 final var bz1 = calibrator.getBiasZAsAcceleration();
24701 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
24702 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24703 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24704 calibrator.getBiasZAsAcceleration(bz2);
24705 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24706 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24707 final var biasTriad1 = calibrator.getBiasAsTriad();
24708 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24709 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24710 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24711 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24712 final var biasTriad2 = new AccelerationTriad();
24713 calibrator.getBiasAsTriad(biasTriad2);
24714 assertEquals(biasTriad1, biasTriad2);
24715 assertEquals(sx, calibrator.getInitialSx(), 0.0);
24716 assertEquals(sy, calibrator.getInitialSy(), 0.0);
24717 assertEquals(sz, calibrator.getInitialSz(), 0.0);
24718 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
24719 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
24720 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
24721 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
24722 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
24723 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
24724 final var bias1 = calibrator.getBias();
24725 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24726 final var bias2 = new double[3];
24727 calibrator.getBias(bias2);
24728 assertArrayEquals(bias1, bias2, 0.0);
24729 final var b1 = calibrator.getBiasAsMatrix();
24730 assertEquals(b1, ba);
24731 final var b2 = new Matrix(3, 1);
24732 calibrator.getBiasAsMatrix(b2);
24733 assertEquals(b1, b2);
24734 final var ma1 = new Matrix(3, 3);
24735 ma1.setSubmatrix(0, 0, 2, 2,
24736 new double[]{sx, myx, mzx,
24737 mxy, sy, mzy,
24738 mxz, myz, sz});
24739 assertEquals(calibrator.getInitialMa(), ma1);
24740 final var ma2 = new Matrix(3, 3);
24741 calibrator.getInitialMa(ma2);
24742 assertEquals(ma1, ma2);
24743 assertSame(measurements, calibrator.getMeasurements());
24744 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
24745 calibrator.getMeasurementType());
24746 assertFalse(calibrator.isOrderedMeasurementsRequired());
24747 assertFalse(calibrator.isQualityScoresRequired());
24748 assertTrue(calibrator.isCommonAxisUsed());
24749 assertNull(calibrator.getListener());
24750 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
24751 assertFalse(calibrator.isReady());
24752 assertFalse(calibrator.isRunning());
24753 assertNull(calibrator.getEstimatedMa());
24754 assertNull(calibrator.getEstimatedSx());
24755 assertNull(calibrator.getEstimatedSy());
24756 assertNull(calibrator.getEstimatedSz());
24757 assertNull(calibrator.getEstimatedMxy());
24758 assertNull(calibrator.getEstimatedMxz());
24759 assertNull(calibrator.getEstimatedMyx());
24760 assertNull(calibrator.getEstimatedMyz());
24761 assertNull(calibrator.getEstimatedMzx());
24762 assertNull(calibrator.getEstimatedMzy());
24763 assertNull(calibrator.getEstimatedCovariance());
24764 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
24765 assertNotNull(calibrator.getGroundTruthGravityNorm());
24766 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24767 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24768 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
24769 ABSOLUTE_ERROR));
24770 final var g = new Acceleration(0.0, AccelerationUnit.G);
24771 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24772 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24773 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
24774
24775
24776 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24777
24778 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
24779 invalidGravityNorm, measurements, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx,
24780 mzy));
24781 }
24782
24783 @Test
24784 void testConstructor210() throws WrongSizeException {
24785 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
24786
24787 final var ba = generateBa();
24788 final var biasX = ba.getElementAtIndex(0);
24789 final var biasY = ba.getElementAtIndex(1);
24790 final var biasZ = ba.getElementAtIndex(2);
24791
24792 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24793 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24794 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24795
24796 final var ma = generateMaCommonAxis();
24797 final var sx = ma.getElementAt(0, 0);
24798 final var sy = ma.getElementAt(1, 1);
24799 final var sz = ma.getElementAt(2, 2);
24800 final var mxy = ma.getElementAt(0, 1);
24801 final var mxz = ma.getElementAt(0, 2);
24802 final var myx = ma.getElementAt(1, 0);
24803 final var myz = ma.getElementAt(1, 2);
24804 final var mzx = ma.getElementAt(2, 0);
24805 final var mzy = ma.getElementAt(2, 1);
24806
24807 final var randomizer = new UniformRandomizer();
24808 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24809 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24810 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24811 final var nedPosition = new NEDPosition(latitude, longitude, height);
24812 final var nedVelocity = new NEDVelocity();
24813 final var ecefPosition = new ECEFPosition();
24814 final var ecefVelocity = new ECEFVelocity();
24815 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
24816 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24817 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24818 final var gravityNorm = gravity.getNormAsAcceleration();
24819
24820 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
24821 true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
24822
24823
24824 assertEquals(biasX, calibrator.getBiasX(), 0.0);
24825 assertEquals(biasY, calibrator.getBiasY(), 0.0);
24826 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
24827 final var bx1 = calibrator.getBiasXAsAcceleration();
24828 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
24829 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
24830 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24831 calibrator.getBiasXAsAcceleration(bx2);
24832 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
24833 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
24834 final var by1 = calibrator.getBiasYAsAcceleration();
24835 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
24836 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
24837 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24838 calibrator.getBiasYAsAcceleration(by2);
24839 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24840 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24841 final var bz1 = calibrator.getBiasZAsAcceleration();
24842 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
24843 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24844 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24845 calibrator.getBiasZAsAcceleration(bz2);
24846 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24847 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24848 final var biasTriad1 = calibrator.getBiasAsTriad();
24849 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24850 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24851 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24852 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24853 final var biasTriad2 = new AccelerationTriad();
24854 calibrator.getBiasAsTriad(biasTriad2);
24855 assertEquals(biasTriad1, biasTriad2);
24856 assertEquals(sx, calibrator.getInitialSx(), 0.0);
24857 assertEquals(sy, calibrator.getInitialSy(), 0.0);
24858 assertEquals(sz, calibrator.getInitialSz(), 0.0);
24859 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
24860 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
24861 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
24862 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
24863 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
24864 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
24865 final var bias1 = calibrator.getBias();
24866 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24867 final var bias2 = new double[3];
24868 calibrator.getBias(bias2);
24869 assertArrayEquals(bias1, bias2, 0.0);
24870 final var b1 = calibrator.getBiasAsMatrix();
24871 assertEquals(b1, ba);
24872 final var b2 = new Matrix(3, 1);
24873 calibrator.getBiasAsMatrix(b2);
24874 assertEquals(b1, b2);
24875 final var ma1 = new Matrix(3, 3);
24876 ma1.setSubmatrix(0, 0, 2, 2,
24877 new double[]{sx, myx, mzx,
24878 mxy, sy, mzy,
24879 mxz, myz, sz});
24880 assertEquals(ma1, calibrator.getInitialMa());
24881 final var ma2 = new Matrix(3, 3);
24882 calibrator.getInitialMa(ma2);
24883 assertEquals(ma1, ma2);
24884 assertSame(measurements, calibrator.getMeasurements());
24885 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
24886 calibrator.getMeasurementType());
24887 assertFalse(calibrator.isOrderedMeasurementsRequired());
24888 assertFalse(calibrator.isQualityScoresRequired());
24889 assertTrue(calibrator.isCommonAxisUsed());
24890 assertSame(this, calibrator.getListener());
24891 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
24892 assertFalse(calibrator.isReady());
24893 assertFalse(calibrator.isRunning());
24894 assertNull(calibrator.getEstimatedMa());
24895 assertNull(calibrator.getEstimatedSx());
24896 assertNull(calibrator.getEstimatedSy());
24897 assertNull(calibrator.getEstimatedSz());
24898 assertNull(calibrator.getEstimatedMxy());
24899 assertNull(calibrator.getEstimatedMxz());
24900 assertNull(calibrator.getEstimatedMyx());
24901 assertNull(calibrator.getEstimatedMyz());
24902 assertNull(calibrator.getEstimatedMzx());
24903 assertNull(calibrator.getEstimatedMzy());
24904 assertNull(calibrator.getEstimatedCovariance());
24905 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
24906 assertNotNull(calibrator.getGroundTruthGravityNorm());
24907 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24908 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24909 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
24910 ABSOLUTE_ERROR));
24911 final var g = new Acceleration(0.0, AccelerationUnit.G);
24912 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24913 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24914 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
24915
24916
24917 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24918
24919 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
24920 invalidGravityNorm, measurements, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx,
24921 mzy, this));
24922 }
24923
24924 @Test
24925 void testConstructor211() throws WrongSizeException {
24926 final var ba = generateBa();
24927 final var bias = ba.getBuffer();
24928 final var biasX = ba.getElementAtIndex(0);
24929 final var biasY = ba.getElementAtIndex(1);
24930 final var biasZ = ba.getElementAtIndex(2);
24931
24932 final var randomizer = new UniformRandomizer();
24933 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24934 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24935 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24936 final var nedPosition = new NEDPosition(latitude, longitude, height);
24937 final var nedVelocity = new NEDVelocity();
24938 final var ecefPosition = new ECEFPosition();
24939 final var ecefVelocity = new ECEFVelocity();
24940 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
24941 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24942 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24943 final var gravityNorm = gravity.getNormAsAcceleration();
24944
24945 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bias);
24946
24947
24948 assertEquals(biasX, calibrator.getBiasX(), 0.0);
24949 assertEquals(biasY, calibrator.getBiasY(), 0.0);
24950 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
24951 final var bx1 = calibrator.getBiasXAsAcceleration();
24952 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
24953 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
24954 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24955 calibrator.getBiasXAsAcceleration(bx2);
24956 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
24957 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
24958 final var by1 = calibrator.getBiasYAsAcceleration();
24959 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24960 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
24961 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24962 calibrator.getBiasYAsAcceleration(by2);
24963 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24964 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24965 final var bz1 = calibrator.getBiasZAsAcceleration();
24966 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24967 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24968 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24969 calibrator.getBiasZAsAcceleration(bz2);
24970 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24971 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24972 final var biasTriad1 = calibrator.getBiasAsTriad();
24973 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24974 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24975 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24976 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24977 final var biasTriad2 = new AccelerationTriad();
24978 calibrator.getBiasAsTriad(biasTriad2);
24979 assertEquals(biasTriad1, biasTriad2);
24980 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
24981 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
24982 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
24983 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
24984 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
24985 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
24986 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
24987 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
24988 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
24989 final var bias1 = calibrator.getBias();
24990 assertArrayEquals(bias1, bias, 0.0);
24991 final var bias2 = new double[3];
24992 calibrator.getBias(bias2);
24993 assertArrayEquals(bias1, bias2, 0.0);
24994 final var b1 = calibrator.getBiasAsMatrix();
24995 assertEquals(b1, ba);
24996 final var b2 = new Matrix(3, 1);
24997 calibrator.getBiasAsMatrix(b2);
24998 assertEquals(b1, b2);
24999 final var ma1 = calibrator.getInitialMa();
25000 assertEquals(new Matrix(3, 3), ma1);
25001 final var ma2 = new Matrix(3, 3);
25002 calibrator.getInitialMa(ma2);
25003 assertEquals(ma1, ma2);
25004 assertNull(calibrator.getMeasurements());
25005 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25006 calibrator.getMeasurementType());
25007 assertFalse(calibrator.isOrderedMeasurementsRequired());
25008 assertFalse(calibrator.isQualityScoresRequired());
25009 assertFalse(calibrator.isCommonAxisUsed());
25010 assertNull(calibrator.getListener());
25011 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
25012 assertFalse(calibrator.isReady());
25013 assertFalse(calibrator.isRunning());
25014 assertNull(calibrator.getEstimatedMa());
25015 assertNull(calibrator.getEstimatedSx());
25016 assertNull(calibrator.getEstimatedSy());
25017 assertNull(calibrator.getEstimatedSz());
25018 assertNull(calibrator.getEstimatedMxy());
25019 assertNull(calibrator.getEstimatedMxz());
25020 assertNull(calibrator.getEstimatedMyx());
25021 assertNull(calibrator.getEstimatedMyz());
25022 assertNull(calibrator.getEstimatedMzx());
25023 assertNull(calibrator.getEstimatedMzy());
25024 assertNull(calibrator.getEstimatedCovariance());
25025 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25026 assertNotNull(calibrator.getGroundTruthGravityNorm());
25027 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25028 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25029 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25030 ABSOLUTE_ERROR));
25031 final var g = new Acceleration(0.0, AccelerationUnit.G);
25032 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25033 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25034 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25035
25036
25037 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25038
25039 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25040 invalidGravityNorm, bias));
25041 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25042 gravityNorm, new double[1]));
25043 }
25044
25045 @Test
25046 void testConstructor212() throws WrongSizeException {
25047 final var ba = generateBa();
25048 final var bias = ba.getBuffer();
25049 final var biasX = ba.getElementAtIndex(0);
25050 final var biasY = ba.getElementAtIndex(1);
25051 final var biasZ = ba.getElementAtIndex(2);
25052
25053 final var randomizer = new UniformRandomizer();
25054 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25055 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25056 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25057 final var nedPosition = new NEDPosition(latitude, longitude, height);
25058 final var nedVelocity = new NEDVelocity();
25059 final var ecefPosition = new ECEFPosition();
25060 final var ecefVelocity = new ECEFVelocity();
25061 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25062 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25063 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25064 final var gravityNorm = gravity.getNormAsAcceleration();
25065
25066 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bias, this);
25067
25068
25069 assertEquals(biasX, calibrator.getBiasX(), 0.0);
25070 assertEquals(biasY, calibrator.getBiasY(), 0.0);
25071 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25072 final var bx1 = calibrator.getBiasXAsAcceleration();
25073 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25074 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25075 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25076 calibrator.getBiasXAsAcceleration(bx2);
25077 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25078 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25079 final var by1 = calibrator.getBiasYAsAcceleration();
25080 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
25081 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25082 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25083 calibrator.getBiasYAsAcceleration(by2);
25084 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25085 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25086 final var bz1 = calibrator.getBiasZAsAcceleration();
25087 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25088 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25089 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25090 calibrator.getBiasZAsAcceleration(bz2);
25091 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25092 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25093 final var biasTriad1 = calibrator.getBiasAsTriad();
25094 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
25095 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
25096 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
25097 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
25098 final var biasTriad2 = new AccelerationTriad();
25099 calibrator.getBiasAsTriad(biasTriad2);
25100 assertEquals(biasTriad1, biasTriad2);
25101 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25102 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25103 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25104 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25105 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25106 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25107 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25108 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25109 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25110 final var bias1 = calibrator.getBias();
25111 assertArrayEquals(bias1, bias, 0.0);
25112 final var bias2 = new double[3];
25113 calibrator.getBias(bias2);
25114 assertArrayEquals(bias1, bias2, 0.0);
25115 final var b1 = calibrator.getBiasAsMatrix();
25116 assertEquals(b1, ba);
25117 final var b2 = new Matrix(3, 1);
25118 calibrator.getBiasAsMatrix(b2);
25119 assertEquals(b1, b2);
25120 final var ma1 = calibrator.getInitialMa();
25121 assertEquals(new Matrix(3, 3), ma1);
25122 final var ma2 = new Matrix(3, 3);
25123 calibrator.getInitialMa(ma2);
25124 assertEquals(ma1, ma2);
25125 assertNull(calibrator.getMeasurements());
25126 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25127 calibrator.getMeasurementType());
25128 assertFalse(calibrator.isOrderedMeasurementsRequired());
25129 assertFalse(calibrator.isQualityScoresRequired());
25130 assertFalse(calibrator.isCommonAxisUsed());
25131 assertSame(this, calibrator.getListener());
25132 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
25133 assertFalse(calibrator.isReady());
25134 assertFalse(calibrator.isRunning());
25135 assertNull(calibrator.getEstimatedMa());
25136 assertNull(calibrator.getEstimatedSx());
25137 assertNull(calibrator.getEstimatedSy());
25138 assertNull(calibrator.getEstimatedSz());
25139 assertNull(calibrator.getEstimatedMxy());
25140 assertNull(calibrator.getEstimatedMxz());
25141 assertNull(calibrator.getEstimatedMyx());
25142 assertNull(calibrator.getEstimatedMyz());
25143 assertNull(calibrator.getEstimatedMzx());
25144 assertNull(calibrator.getEstimatedMzy());
25145 assertNull(calibrator.getEstimatedCovariance());
25146 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25147 assertNotNull(calibrator.getGroundTruthGravityNorm());
25148 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25149 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25150 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25151 ABSOLUTE_ERROR));
25152 final var g = new Acceleration(0.0, AccelerationUnit.G);
25153 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25154 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25155 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25156
25157
25158 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25159
25160 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25161 invalidGravityNorm, bias, this));
25162 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25163 gravityNorm, new double[1], this));
25164 }
25165
25166 @Test
25167 void testConstructor213() throws WrongSizeException {
25168 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
25169
25170 final var ba = generateBa();
25171 final var bias = ba.getBuffer();
25172 final var biasX = ba.getElementAtIndex(0);
25173 final var biasY = ba.getElementAtIndex(1);
25174 final var biasZ = ba.getElementAtIndex(2);
25175
25176 final var randomizer = new UniformRandomizer();
25177 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25178 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25179 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25180 final var nedPosition = new NEDPosition(latitude, longitude, height);
25181 final var nedVelocity = new NEDVelocity();
25182 final var ecefPosition = new ECEFPosition();
25183 final var ecefVelocity = new ECEFVelocity();
25184 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25185 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25186 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25187 final var gravityNorm = gravity.getNormAsAcceleration();
25188
25189 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bias);
25190
25191
25192 assertEquals(biasX, calibrator.getBiasX(), 0.0);
25193 assertEquals(biasY, calibrator.getBiasY(), 0.0);
25194 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25195 final var bx1 = calibrator.getBiasXAsAcceleration();
25196 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25197 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25198 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25199 calibrator.getBiasXAsAcceleration(bx2);
25200 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25201 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25202 final var by1 = calibrator.getBiasYAsAcceleration();
25203 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25204 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25205 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25206 calibrator.getBiasYAsAcceleration(by2);
25207 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25208 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25209 final var bz1 = calibrator.getBiasZAsAcceleration();
25210 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25211 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25212 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25213 calibrator.getBiasZAsAcceleration(bz2);
25214 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25215 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25216 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25217 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25218 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25219 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25220 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25221 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25222 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25223 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25224 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25225 final var bias1 = calibrator.getBias();
25226 assertArrayEquals(bias1, bias, 0.0);
25227 final var bias2 = new double[3];
25228 calibrator.getBias(bias2);
25229 assertArrayEquals(bias1, bias2, 0.0);
25230 final var b1 = calibrator.getBiasAsMatrix();
25231 assertEquals(b1, ba);
25232 final var b2 = new Matrix(3, 1);
25233 calibrator.getBiasAsMatrix(b2);
25234 assertEquals(b1, b2);
25235 final var ma1 = calibrator.getInitialMa();
25236 assertEquals(new Matrix(3, 3), ma1);
25237 final var ma2 = new Matrix(3, 3);
25238 calibrator.getInitialMa(ma2);
25239 assertEquals(ma1, ma2);
25240 assertSame(measurements, calibrator.getMeasurements());
25241 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25242 calibrator.getMeasurementType());
25243 assertFalse(calibrator.isOrderedMeasurementsRequired());
25244 assertFalse(calibrator.isQualityScoresRequired());
25245 assertFalse(calibrator.isCommonAxisUsed());
25246 assertNull(calibrator.getListener());
25247 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
25248 assertFalse(calibrator.isReady());
25249 assertFalse(calibrator.isRunning());
25250 assertNull(calibrator.getEstimatedMa());
25251 assertNull(calibrator.getEstimatedSx());
25252 assertNull(calibrator.getEstimatedSy());
25253 assertNull(calibrator.getEstimatedSz());
25254 assertNull(calibrator.getEstimatedMxy());
25255 assertNull(calibrator.getEstimatedMxz());
25256 assertNull(calibrator.getEstimatedMyx());
25257 assertNull(calibrator.getEstimatedMyz());
25258 assertNull(calibrator.getEstimatedMzx());
25259 assertNull(calibrator.getEstimatedMzy());
25260 assertNull(calibrator.getEstimatedCovariance());
25261 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25262 assertNotNull(calibrator.getGroundTruthGravityNorm());
25263 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25264 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25265 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25266 ABSOLUTE_ERROR));
25267 final var g = new Acceleration(0.0, AccelerationUnit.G);
25268 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25269 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25270 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25271
25272
25273 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25274
25275 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25276 invalidGravityNorm, measurements, bias));
25277 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25278 gravityNorm, measurements, new double[1]));
25279 }
25280
25281 @Test
25282 void testConstructor214() throws WrongSizeException {
25283 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
25284
25285 final var ba = generateBa();
25286 final var bias = ba.getBuffer();
25287 final var biasX = ba.getElementAtIndex(0);
25288 final var biasY = ba.getElementAtIndex(1);
25289 final var biasZ = ba.getElementAtIndex(2);
25290
25291 final var randomizer = new UniformRandomizer();
25292 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25293 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25294 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25295 final var nedPosition = new NEDPosition(latitude, longitude, height);
25296 final var nedVelocity = new NEDVelocity();
25297 final var ecefPosition = new ECEFPosition();
25298 final var ecefVelocity = new ECEFVelocity();
25299 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25300 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25301 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25302 final var gravityNorm = gravity.getNormAsAcceleration();
25303
25304 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bias,
25305 this);
25306
25307
25308 assertEquals(biasX, calibrator.getBiasX(), 0.0);
25309 assertEquals(biasY, calibrator.getBiasY(), 0.0);
25310 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25311 final var bx1 = calibrator.getBiasXAsAcceleration();
25312 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25313 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25314 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25315 calibrator.getBiasXAsAcceleration(bx2);
25316 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25317 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25318 final var by1 = calibrator.getBiasYAsAcceleration();
25319 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
25320 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25321 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25322 calibrator.getBiasYAsAcceleration(by2);
25323 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25324 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25325 final var bz1 = calibrator.getBiasZAsAcceleration();
25326 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25327 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25328 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25329 calibrator.getBiasZAsAcceleration(bz2);
25330 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25331 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25332 final var biasTriad1 = calibrator.getBiasAsTriad();
25333 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
25334 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
25335 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
25336 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
25337 final var biasTriad2 = new AccelerationTriad();
25338 calibrator.getBiasAsTriad(biasTriad2);
25339 assertEquals(biasTriad1, biasTriad2);
25340 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25341 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25342 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25343 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25344 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25345 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25346 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25347 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25348 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25349 final var bias1 = calibrator.getBias();
25350 assertArrayEquals(bias1, bias, 0.0);
25351 final var bias2 = new double[3];
25352 calibrator.getBias(bias2);
25353 assertArrayEquals(bias1, bias2, 0.0);
25354 final var b1 = calibrator.getBiasAsMatrix();
25355 assertEquals(b1, ba);
25356 final var b2 = new Matrix(3, 1);
25357 calibrator.getBiasAsMatrix(b2);
25358 assertEquals(b1, b2);
25359 final var ma1 = calibrator.getInitialMa();
25360 assertEquals(new Matrix(3, 3), ma1);
25361 final var ma2 = new Matrix(3, 3);
25362 calibrator.getInitialMa(ma2);
25363 assertEquals(ma1, ma2);
25364 assertSame(measurements, calibrator.getMeasurements());
25365 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25366 calibrator.getMeasurementType());
25367 assertFalse(calibrator.isOrderedMeasurementsRequired());
25368 assertFalse(calibrator.isQualityScoresRequired());
25369 assertFalse(calibrator.isCommonAxisUsed());
25370 assertSame(this, calibrator.getListener());
25371 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
25372 assertFalse(calibrator.isReady());
25373 assertFalse(calibrator.isRunning());
25374 assertNull(calibrator.getEstimatedMa());
25375 assertNull(calibrator.getEstimatedSx());
25376 assertNull(calibrator.getEstimatedSy());
25377 assertNull(calibrator.getEstimatedSz());
25378 assertNull(calibrator.getEstimatedMxy());
25379 assertNull(calibrator.getEstimatedMxz());
25380 assertNull(calibrator.getEstimatedMyx());
25381 assertNull(calibrator.getEstimatedMyz());
25382 assertNull(calibrator.getEstimatedMzx());
25383 assertNull(calibrator.getEstimatedMzy());
25384 assertNull(calibrator.getEstimatedCovariance());
25385 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25386 assertNotNull(calibrator.getGroundTruthGravityNorm());
25387 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25388 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25389 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25390 ABSOLUTE_ERROR));
25391 final var g = new Acceleration(0.0, AccelerationUnit.G);
25392 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25393 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25394 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25395
25396
25397 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25398
25399 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25400 invalidGravityNorm, measurements, bias, this));
25401 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25402 gravityNorm, measurements, new double[1], this));
25403 }
25404
25405 @Test
25406 void testConstructor215() throws WrongSizeException {
25407 final var ba = generateBa();
25408 final var bias = ba.getBuffer();
25409 final var biasX = ba.getElementAtIndex(0);
25410 final var biasY = ba.getElementAtIndex(1);
25411 final var biasZ = ba.getElementAtIndex(2);
25412
25413 final var randomizer = new UniformRandomizer();
25414 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25415 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25416 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25417 final var nedPosition = new NEDPosition(latitude, longitude, height);
25418 final var nedVelocity = new NEDVelocity();
25419 final var ecefPosition = new ECEFPosition();
25420 final var ecefVelocity = new ECEFVelocity();
25421 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25422 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25423 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25424 final var gravityNorm = gravity.getNormAsAcceleration();
25425
25426 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
25427 bias);
25428
25429
25430 assertEquals(biasX, calibrator.getBiasX(), 0.0);
25431 assertEquals(biasY, calibrator.getBiasY(), 0.0);
25432 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25433 final var bx1 = calibrator.getBiasXAsAcceleration();
25434 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25435 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25436 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25437 calibrator.getBiasXAsAcceleration(bx2);
25438 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25439 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25440 final var by1 = calibrator.getBiasYAsAcceleration();
25441 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
25442 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25443 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25444 calibrator.getBiasYAsAcceleration(by2);
25445 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25446 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25447 final var bz1 = calibrator.getBiasZAsAcceleration();
25448 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25449 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25450 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25451 calibrator.getBiasZAsAcceleration(bz2);
25452 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25453 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25454 final var biasTriad1 = calibrator.getBiasAsTriad();
25455 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
25456 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
25457 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
25458 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
25459 final var biasTriad2 = new AccelerationTriad();
25460 calibrator.getBiasAsTriad(biasTriad2);
25461 assertEquals(biasTriad1, biasTriad2);
25462 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25463 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25464 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25465 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25466 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25467 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25468 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25469 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25470 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25471 final var bias1 = calibrator.getBias();
25472 assertArrayEquals(bias1, bias, 0.0);
25473 final var bias2 = new double[3];
25474 calibrator.getBias(bias2);
25475 assertArrayEquals(bias1, bias2, 0.0);
25476 final var b1 = calibrator.getBiasAsMatrix();
25477 assertEquals(b1, ba);
25478 final var b2 = new Matrix(3, 1);
25479 calibrator.getBiasAsMatrix(b2);
25480 assertEquals(b1, b2);
25481 final var ma1 = calibrator.getInitialMa();
25482 assertEquals(new Matrix(3, 3), ma1);
25483 final var ma2 = new Matrix(3, 3);
25484 calibrator.getInitialMa(ma2);
25485 assertEquals(ma1, ma2);
25486 assertNull(calibrator.getMeasurements());
25487 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25488 calibrator.getMeasurementType());
25489 assertFalse(calibrator.isOrderedMeasurementsRequired());
25490 assertFalse(calibrator.isQualityScoresRequired());
25491 assertTrue(calibrator.isCommonAxisUsed());
25492 assertNull(calibrator.getListener());
25493 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
25494 assertFalse(calibrator.isReady());
25495 assertFalse(calibrator.isRunning());
25496 assertNull(calibrator.getEstimatedMa());
25497 assertNull(calibrator.getEstimatedSx());
25498 assertNull(calibrator.getEstimatedSy());
25499 assertNull(calibrator.getEstimatedSz());
25500 assertNull(calibrator.getEstimatedMxy());
25501 assertNull(calibrator.getEstimatedMxz());
25502 assertNull(calibrator.getEstimatedMyx());
25503 assertNull(calibrator.getEstimatedMyz());
25504 assertNull(calibrator.getEstimatedMzx());
25505 assertNull(calibrator.getEstimatedMzy());
25506 assertNull(calibrator.getEstimatedCovariance());
25507 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25508 assertNotNull(calibrator.getGroundTruthGravityNorm());
25509 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25510 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25511 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25512 ABSOLUTE_ERROR));
25513 final var g = new Acceleration(0.0, AccelerationUnit.G);
25514 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25515 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25516 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25517
25518
25519 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25520
25521 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25522 invalidGravityNorm, true, bias));
25523 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25524 gravityNorm, true, new double[1]));
25525 }
25526
25527 @Test
25528 void testConstructor216() throws WrongSizeException {
25529 final var ba = generateBa();
25530 final var bias = ba.getBuffer();
25531 final var biasX = ba.getElementAtIndex(0);
25532 final var biasY = ba.getElementAtIndex(1);
25533 final var biasZ = ba.getElementAtIndex(2);
25534
25535 final var randomizer = new UniformRandomizer();
25536 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25537 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25538 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25539 final var nedPosition = new NEDPosition(latitude, longitude, height);
25540 final var nedVelocity = new NEDVelocity();
25541 final var ecefPosition = new ECEFPosition();
25542 final var ecefVelocity = new ECEFVelocity();
25543 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25544 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25545 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25546 final var gravityNorm = gravity.getNormAsAcceleration();
25547
25548 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
25549 bias, this);
25550
25551
25552 assertEquals(biasX, calibrator.getBiasX(), 0.0);
25553 assertEquals(biasY, calibrator.getBiasY(), 0.0);
25554 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25555 final var bx1 = calibrator.getBiasXAsAcceleration();
25556 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25557 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25558 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25559 calibrator.getBiasXAsAcceleration(bx2);
25560 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25561 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25562 final var by1 = calibrator.getBiasYAsAcceleration();
25563 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
25564 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25565 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25566 calibrator.getBiasYAsAcceleration(by2);
25567 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25568 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25569 final var bz1 = calibrator.getBiasZAsAcceleration();
25570 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25571 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25572 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25573 calibrator.getBiasZAsAcceleration(bz2);
25574 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25575 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25576 final var biasTriad1 = calibrator.getBiasAsTriad();
25577 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
25578 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
25579 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
25580 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
25581 final var biasTriad2 = new AccelerationTriad();
25582 calibrator.getBiasAsTriad(biasTriad2);
25583 assertEquals(biasTriad1, biasTriad2);
25584 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25585 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25586 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25587 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25588 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25589 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25590 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25591 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25592 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25593 final var bias1 = calibrator.getBias();
25594 assertArrayEquals(bias1, bias, 0.0);
25595 final var bias2 = new double[3];
25596 calibrator.getBias(bias2);
25597 assertArrayEquals(bias1, bias2, 0.0);
25598 final var b1 = calibrator.getBiasAsMatrix();
25599 assertEquals(b1, ba);
25600 final var b2 = new Matrix(3, 1);
25601 calibrator.getBiasAsMatrix(b2);
25602 assertEquals(b1, b2);
25603 final var ma1 = calibrator.getInitialMa();
25604 assertEquals(new Matrix(3, 3), ma1);
25605 final var ma2 = new Matrix(3, 3);
25606 calibrator.getInitialMa(ma2);
25607 assertEquals(ma1, ma2);
25608 assertNull(calibrator.getMeasurements());
25609 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25610 calibrator.getMeasurementType());
25611 assertFalse(calibrator.isOrderedMeasurementsRequired());
25612 assertFalse(calibrator.isQualityScoresRequired());
25613 assertTrue(calibrator.isCommonAxisUsed());
25614 assertSame(this, calibrator.getListener());
25615 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
25616 assertFalse(calibrator.isReady());
25617 assertFalse(calibrator.isRunning());
25618 assertNull(calibrator.getEstimatedMa());
25619 assertNull(calibrator.getEstimatedSx());
25620 assertNull(calibrator.getEstimatedSy());
25621 assertNull(calibrator.getEstimatedSz());
25622 assertNull(calibrator.getEstimatedMxy());
25623 assertNull(calibrator.getEstimatedMxz());
25624 assertNull(calibrator.getEstimatedMyx());
25625 assertNull(calibrator.getEstimatedMyz());
25626 assertNull(calibrator.getEstimatedMzx());
25627 assertNull(calibrator.getEstimatedMzy());
25628 assertNull(calibrator.getEstimatedCovariance());
25629 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25630 assertNotNull(calibrator.getGroundTruthGravityNorm());
25631 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25632 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25633 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25634 ABSOLUTE_ERROR));
25635 final var g = new Acceleration(0.0, AccelerationUnit.G);
25636 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25637 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25638 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25639
25640
25641 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25642
25643 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25644 invalidGravityNorm, true, bias, this));
25645 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25646 gravityNorm, true, new double[1], this));
25647 }
25648
25649 @Test
25650 void testConstructor217() throws WrongSizeException {
25651 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
25652
25653 final var ba = generateBa();
25654 final var bias = ba.getBuffer();
25655 final var biasX = ba.getElementAtIndex(0);
25656 final var biasY = ba.getElementAtIndex(1);
25657 final var biasZ = ba.getElementAtIndex(2);
25658
25659 final var randomizer = new UniformRandomizer();
25660 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25661 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25662 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25663 final var nedPosition = new NEDPosition(latitude, longitude, height);
25664 final var nedVelocity = new NEDVelocity();
25665 final var ecefPosition = new ECEFPosition();
25666 final var ecefVelocity = new ECEFVelocity();
25667 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25668 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25669 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25670 final var gravityNorm = gravity.getNormAsAcceleration();
25671
25672 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
25673 true, bias);
25674
25675
25676 assertEquals(biasX, calibrator.getBiasX(), 0.0);
25677 assertEquals(biasY, calibrator.getBiasY(), 0.0);
25678 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25679 final var bx1 = calibrator.getBiasXAsAcceleration();
25680 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25681 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25682 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25683 calibrator.getBiasXAsAcceleration(bx2);
25684 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25685 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25686 final var by1 = calibrator.getBiasYAsAcceleration();
25687 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
25688 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25689 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25690 calibrator.getBiasYAsAcceleration(by2);
25691 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25692 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25693 final var bz1 = calibrator.getBiasZAsAcceleration();
25694 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25695 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25696 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25697 calibrator.getBiasZAsAcceleration(bz2);
25698 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25699 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25700 final var biasTriad1 = calibrator.getBiasAsTriad();
25701 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
25702 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
25703 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
25704 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
25705 final var biasTriad2 = new AccelerationTriad();
25706 calibrator.getBiasAsTriad(biasTriad2);
25707 assertEquals(biasTriad1, biasTriad2);
25708 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25709 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25710 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25711 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25712 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25713 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25714 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25715 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25716 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25717 final var bias1 = calibrator.getBias();
25718 assertArrayEquals(bias1, bias, 0.0);
25719 final var bias2 = new double[3];
25720 calibrator.getBias(bias2);
25721 assertArrayEquals(bias1, bias2, 0.0);
25722 final var b1 = calibrator.getBiasAsMatrix();
25723 assertEquals(b1, ba);
25724 final var b2 = new Matrix(3, 1);
25725 calibrator.getBiasAsMatrix(b2);
25726 assertEquals(b1, b2);
25727 final var ma1 = calibrator.getInitialMa();
25728 assertEquals(new Matrix(3, 3), ma1);
25729 final var ma2 = new Matrix(3, 3);
25730 calibrator.getInitialMa(ma2);
25731 assertEquals(ma1, ma2);
25732 assertSame(measurements, calibrator.getMeasurements());
25733 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25734 calibrator.getMeasurementType());
25735 assertFalse(calibrator.isOrderedMeasurementsRequired());
25736 assertFalse(calibrator.isQualityScoresRequired());
25737 assertTrue(calibrator.isCommonAxisUsed());
25738 assertNull(calibrator.getListener());
25739 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
25740 assertFalse(calibrator.isReady());
25741 assertFalse(calibrator.isRunning());
25742 assertNull(calibrator.getEstimatedMa());
25743 assertNull(calibrator.getEstimatedSx());
25744 assertNull(calibrator.getEstimatedSy());
25745 assertNull(calibrator.getEstimatedSz());
25746 assertNull(calibrator.getEstimatedMxy());
25747 assertNull(calibrator.getEstimatedMxz());
25748 assertNull(calibrator.getEstimatedMyx());
25749 assertNull(calibrator.getEstimatedMyz());
25750 assertNull(calibrator.getEstimatedMzx());
25751 assertNull(calibrator.getEstimatedMzy());
25752 assertNull(calibrator.getEstimatedCovariance());
25753 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25754 assertNotNull(calibrator.getGroundTruthGravityNorm());
25755 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25756 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25757 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25758 ABSOLUTE_ERROR));
25759 final var g = new Acceleration(0.0, AccelerationUnit.G);
25760 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25761 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25762 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25763
25764
25765 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25766
25767 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25768 invalidGravityNorm, measurements, true, bias));
25769 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25770 gravityNorm, measurements, true, new double[1]));
25771 }
25772
25773 @Test
25774 void testConstructor218() throws WrongSizeException {
25775 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
25776
25777 final var ba = generateBa();
25778 final var bias = ba.getBuffer();
25779 final var biasX = ba.getElementAtIndex(0);
25780 final var biasY = ba.getElementAtIndex(1);
25781 final var biasZ = ba.getElementAtIndex(2);
25782
25783 final var randomizer = new UniformRandomizer();
25784 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25785 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25786 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25787 final var nedPosition = new NEDPosition(latitude, longitude, height);
25788 final var nedVelocity = new NEDVelocity();
25789 final var ecefPosition = new ECEFPosition();
25790 final var ecefVelocity = new ECEFVelocity();
25791 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25792 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25793 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25794 final var gravityNorm = gravity.getNormAsAcceleration();
25795
25796 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
25797 true, bias, this);
25798
25799
25800 assertEquals(biasX, calibrator.getBiasX(), 0.0);
25801 assertEquals(biasY, calibrator.getBiasY(), 0.0);
25802 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25803 final var bx1 = calibrator.getBiasXAsAcceleration();
25804 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25805 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25806 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25807 calibrator.getBiasXAsAcceleration(bx2);
25808 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25809 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25810 final var by1 = calibrator.getBiasYAsAcceleration();
25811 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
25812 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25813 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25814 calibrator.getBiasYAsAcceleration(by2);
25815 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25816 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25817 final var bz1 = calibrator.getBiasZAsAcceleration();
25818 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25819 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25820 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25821 calibrator.getBiasZAsAcceleration(bz2);
25822 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25823 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25824 final var biasTriad1 = calibrator.getBiasAsTriad();
25825 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
25826 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
25827 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
25828 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
25829 final var biasTriad2 = new AccelerationTriad();
25830 calibrator.getBiasAsTriad(biasTriad2);
25831 assertEquals(biasTriad1, biasTriad2);
25832 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25833 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25834 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25835 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25836 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25837 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25838 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25839 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25840 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25841 final var bias1 = calibrator.getBias();
25842 assertArrayEquals(bias1, bias, 0.0);
25843 final var bias2 = new double[3];
25844 calibrator.getBias(bias2);
25845 assertArrayEquals(bias1, bias2, 0.0);
25846 final var b1 = calibrator.getBiasAsMatrix();
25847 assertEquals(b1, ba);
25848 final var b2 = new Matrix(3, 1);
25849 calibrator.getBiasAsMatrix(b2);
25850 assertEquals(b1, b2);
25851 final var ma1 = calibrator.getInitialMa();
25852 assertEquals(new Matrix(3, 3), ma1);
25853 final var ma2 = new Matrix(3, 3);
25854 calibrator.getInitialMa(ma2);
25855 assertEquals(ma1, ma2);
25856 assertSame(measurements, calibrator.getMeasurements());
25857 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25858 calibrator.getMeasurementType());
25859 assertFalse(calibrator.isOrderedMeasurementsRequired());
25860 assertFalse(calibrator.isQualityScoresRequired());
25861 assertTrue(calibrator.isCommonAxisUsed());
25862 assertSame(this, calibrator.getListener());
25863 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
25864 assertFalse(calibrator.isReady());
25865 assertFalse(calibrator.isRunning());
25866 assertNull(calibrator.getEstimatedMa());
25867 assertNull(calibrator.getEstimatedSx());
25868 assertNull(calibrator.getEstimatedSy());
25869 assertNull(calibrator.getEstimatedSz());
25870 assertNull(calibrator.getEstimatedMxy());
25871 assertNull(calibrator.getEstimatedMxz());
25872 assertNull(calibrator.getEstimatedMyx());
25873 assertNull(calibrator.getEstimatedMyz());
25874 assertNull(calibrator.getEstimatedMzx());
25875 assertNull(calibrator.getEstimatedMzy());
25876 assertNull(calibrator.getEstimatedCovariance());
25877 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25878 assertNotNull(calibrator.getGroundTruthGravityNorm());
25879 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25880 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25881 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25882 ABSOLUTE_ERROR));
25883 final var g = new Acceleration(0.0, AccelerationUnit.G);
25884 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25885 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25886 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25887
25888
25889 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25890
25891 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25892 invalidGravityNorm, measurements, true, bias, this));
25893 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25894 gravityNorm, measurements, true, new double[1], this));
25895 }
25896
25897 @Test
25898 void testConstructor219() throws WrongSizeException {
25899 final var ba = generateBa();
25900 final var bias = ba.getBuffer();
25901 final var biasX = ba.getElementAtIndex(0);
25902 final var biasY = ba.getElementAtIndex(1);
25903 final var biasZ = ba.getElementAtIndex(2);
25904
25905 final var randomizer = new UniformRandomizer();
25906 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25907 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25908 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25909 final var nedPosition = new NEDPosition(latitude, longitude, height);
25910 final var nedVelocity = new NEDVelocity();
25911 final var ecefPosition = new ECEFPosition();
25912 final var ecefVelocity = new ECEFVelocity();
25913 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25914 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25915 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25916 final var gravityNorm = gravity.getNormAsAcceleration();
25917
25918 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba);
25919
25920
25921 assertEquals(biasX, calibrator.getBiasX(), 0.0);
25922 assertEquals(biasY, calibrator.getBiasY(), 0.0);
25923 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25924 final var bx1 = calibrator.getBiasXAsAcceleration();
25925 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25926 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25927 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25928 calibrator.getBiasXAsAcceleration(bx2);
25929 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25930 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25931 final var by1 = calibrator.getBiasYAsAcceleration();
25932 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
25933 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25934 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25935 calibrator.getBiasYAsAcceleration(by2);
25936 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25937 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25938 final var bz1 = calibrator.getBiasZAsAcceleration();
25939 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25940 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25941 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25942 calibrator.getBiasZAsAcceleration(bz2);
25943 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25944 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25945 final var biasTriad1 = calibrator.getBiasAsTriad();
25946 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
25947 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
25948 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
25949 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
25950 final var biasTriad2 = new AccelerationTriad();
25951 calibrator.getBiasAsTriad(biasTriad2);
25952 assertEquals(biasTriad1, biasTriad2);
25953 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25954 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25955 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25956 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25957 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25958 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25959 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25960 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25961 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25962 final var bias1 = calibrator.getBias();
25963 assertArrayEquals(bias1, bias, 0.0);
25964 final var bias2 = new double[3];
25965 calibrator.getBias(bias2);
25966 assertArrayEquals(bias1, bias2, 0.0);
25967 final var b1 = calibrator.getBiasAsMatrix();
25968 assertEquals(b1, ba);
25969 final var b2 = new Matrix(3, 1);
25970 calibrator.getBiasAsMatrix(b2);
25971 assertEquals(b1, b2);
25972 final var ma1 = calibrator.getInitialMa();
25973 assertEquals(new Matrix(3, 3), ma1);
25974 final var ma2 = new Matrix(3, 3);
25975 calibrator.getInitialMa(ma2);
25976 assertEquals(ma1, ma2);
25977 assertNull(calibrator.getMeasurements());
25978 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25979 calibrator.getMeasurementType());
25980 assertFalse(calibrator.isOrderedMeasurementsRequired());
25981 assertFalse(calibrator.isQualityScoresRequired());
25982 assertFalse(calibrator.isCommonAxisUsed());
25983 assertNull(calibrator.getListener());
25984 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
25985 assertFalse(calibrator.isReady());
25986 assertFalse(calibrator.isRunning());
25987 assertNull(calibrator.getEstimatedMa());
25988 assertNull(calibrator.getEstimatedSx());
25989 assertNull(calibrator.getEstimatedSy());
25990 assertNull(calibrator.getEstimatedSz());
25991 assertNull(calibrator.getEstimatedMxy());
25992 assertNull(calibrator.getEstimatedMxz());
25993 assertNull(calibrator.getEstimatedMyx());
25994 assertNull(calibrator.getEstimatedMyz());
25995 assertNull(calibrator.getEstimatedMzx());
25996 assertNull(calibrator.getEstimatedMzy());
25997 assertNull(calibrator.getEstimatedCovariance());
25998 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25999 assertNotNull(calibrator.getGroundTruthGravityNorm());
26000 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26001 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26002 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26003 ABSOLUTE_ERROR));
26004 final var g = new Acceleration(0.0, AccelerationUnit.G);
26005 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26006 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26007 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26008
26009
26010 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26011
26012 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26013 invalidGravityNorm, ba));
26014 final var m1 = new Matrix(1, 1);
26015 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26016 gravityNorm, m1));
26017 final var m2 = new Matrix(1, 3);
26018 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26019 gravityNorm, m2));
26020 }
26021
26022 @Test
26023 void testConstructor220() throws WrongSizeException {
26024 final var ba = generateBa();
26025 final var bias = ba.getBuffer();
26026 final var biasX = ba.getElementAtIndex(0);
26027 final var biasY = ba.getElementAtIndex(1);
26028 final var biasZ = ba.getElementAtIndex(2);
26029
26030 final var randomizer = new UniformRandomizer();
26031 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26032 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26033 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26034 final var nedPosition = new NEDPosition(latitude, longitude, height);
26035 final var nedVelocity = new NEDVelocity();
26036 final var ecefPosition = new ECEFPosition();
26037 final var ecefVelocity = new ECEFVelocity();
26038 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26039 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26040 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26041 final var gravityNorm = gravity.getNormAsAcceleration();
26042
26043 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba, this);
26044
26045
26046 assertEquals(biasX, calibrator.getBiasX(), 0.0);
26047 assertEquals(biasY, calibrator.getBiasY(), 0.0);
26048 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26049 final var bx1 = calibrator.getBiasXAsAcceleration();
26050 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26051 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26052 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26053 calibrator.getBiasXAsAcceleration(bx2);
26054 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26055 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26056 final var by1 = calibrator.getBiasYAsAcceleration();
26057 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26058 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26059 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26060 calibrator.getBiasYAsAcceleration(by2);
26061 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26062 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26063 final var bz1 = calibrator.getBiasZAsAcceleration();
26064 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26065 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26066 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26067 calibrator.getBiasZAsAcceleration(bz2);
26068 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26069 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26070 final var biasTriad1 = calibrator.getBiasAsTriad();
26071 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26072 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26073 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26074 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26075 final var biasTriad2 = new AccelerationTriad();
26076 calibrator.getBiasAsTriad(biasTriad2);
26077 assertEquals(biasTriad1, biasTriad2);
26078 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
26079 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
26080 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
26081 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
26082 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
26083 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
26084 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
26085 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
26086 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
26087 final var bias1 = calibrator.getBias();
26088 assertArrayEquals(bias1, bias, 0.0);
26089 final var bias2 = new double[3];
26090 calibrator.getBias(bias2);
26091 assertArrayEquals(bias1, bias2, 0.0);
26092 final var b1 = calibrator.getBiasAsMatrix();
26093 assertEquals(b1, ba);
26094 final var b2 = new Matrix(3, 1);
26095 calibrator.getBiasAsMatrix(b2);
26096 assertEquals(b1, b2);
26097 final var ma1 = calibrator.getInitialMa();
26098 assertEquals(new Matrix(3, 3), ma1);
26099 final var ma2 = new Matrix(3, 3);
26100 calibrator.getInitialMa(ma2);
26101 assertEquals(ma1, ma2);
26102 assertNull(calibrator.getMeasurements());
26103 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
26104 calibrator.getMeasurementType());
26105 assertFalse(calibrator.isOrderedMeasurementsRequired());
26106 assertFalse(calibrator.isQualityScoresRequired());
26107 assertFalse(calibrator.isCommonAxisUsed());
26108 assertSame(this, calibrator.getListener());
26109 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
26110 assertFalse(calibrator.isReady());
26111 assertFalse(calibrator.isRunning());
26112 assertNull(calibrator.getEstimatedMa());
26113 assertNull(calibrator.getEstimatedSx());
26114 assertNull(calibrator.getEstimatedSy());
26115 assertNull(calibrator.getEstimatedSz());
26116 assertNull(calibrator.getEstimatedMxy());
26117 assertNull(calibrator.getEstimatedMxz());
26118 assertNull(calibrator.getEstimatedMyx());
26119 assertNull(calibrator.getEstimatedMyz());
26120 assertNull(calibrator.getEstimatedMzx());
26121 assertNull(calibrator.getEstimatedMzy());
26122 assertNull(calibrator.getEstimatedCovariance());
26123 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
26124 assertNotNull(calibrator.getGroundTruthGravityNorm());
26125 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26126 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26127 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26128 ABSOLUTE_ERROR));
26129 final var g = new Acceleration(0.0, AccelerationUnit.G);
26130 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26131 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26132 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26133
26134
26135 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26136
26137 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26138 invalidGravityNorm, ba));
26139 final var m1 = new Matrix(1, 1);
26140 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26141 gravityNorm, m1, this));
26142 final var m2 = new Matrix(1, 3);
26143 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26144 gravityNorm, m2, this));
26145 }
26146
26147 @Test
26148 void testConstructor221() throws WrongSizeException {
26149 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
26150
26151 final var ba = generateBa();
26152 final var bias = ba.getBuffer();
26153 final var biasX = ba.getElementAtIndex(0);
26154 final var biasY = ba.getElementAtIndex(1);
26155 final var biasZ = ba.getElementAtIndex(2);
26156
26157 final var randomizer = new UniformRandomizer();
26158 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26159 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26160 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26161 final var nedPosition = new NEDPosition(latitude, longitude, height);
26162 final var nedVelocity = new NEDVelocity();
26163 final var ecefPosition = new ECEFPosition();
26164 final var ecefVelocity = new ECEFVelocity();
26165 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26166 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26167 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26168 final var gravityNorm = gravity.getNormAsAcceleration();
26169
26170 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba);
26171
26172
26173 assertEquals(biasX, calibrator.getBiasX(), 0.0);
26174 assertEquals(biasY, calibrator.getBiasY(), 0.0);
26175 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26176 final var bx1 = calibrator.getBiasXAsAcceleration();
26177 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26178 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26179 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26180 calibrator.getBiasXAsAcceleration(bx2);
26181 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26182 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26183 final var by1 = calibrator.getBiasYAsAcceleration();
26184 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26185 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26186 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26187 calibrator.getBiasYAsAcceleration(by2);
26188 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26189 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26190 final var bz1 = calibrator.getBiasZAsAcceleration();
26191 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26192 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26193 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26194 calibrator.getBiasZAsAcceleration(bz2);
26195 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26196 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26197 final var biasTriad1 = calibrator.getBiasAsTriad();
26198 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26199 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26200 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26201 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26202 final var biasTriad2 = new AccelerationTriad();
26203 calibrator.getBiasAsTriad(biasTriad2);
26204 assertEquals(biasTriad1, biasTriad2);
26205 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
26206 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
26207 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
26208 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
26209 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
26210 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
26211 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
26212 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
26213 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
26214 final var bias1 = calibrator.getBias();
26215 assertArrayEquals(bias1, bias, 0.0);
26216 final var bias2 = new double[3];
26217 calibrator.getBias(bias2);
26218 assertArrayEquals(bias1, bias2, 0.0);
26219 final var b1 = calibrator.getBiasAsMatrix();
26220 assertEquals(b1, ba);
26221 final var b2 = new Matrix(3, 1);
26222 calibrator.getBiasAsMatrix(b2);
26223 assertEquals(b1, b2);
26224 final var ma1 = calibrator.getInitialMa();
26225 assertEquals(new Matrix(3, 3), ma1);
26226 final var ma2 = new Matrix(3, 3);
26227 calibrator.getInitialMa(ma2);
26228 assertEquals(ma1, ma2);
26229 assertSame(measurements, calibrator.getMeasurements());
26230 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
26231 calibrator.getMeasurementType());
26232 assertFalse(calibrator.isOrderedMeasurementsRequired());
26233 assertFalse(calibrator.isQualityScoresRequired());
26234 assertFalse(calibrator.isCommonAxisUsed());
26235 assertNull(calibrator.getListener());
26236 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
26237 assertFalse(calibrator.isReady());
26238 assertFalse(calibrator.isRunning());
26239 assertNull(calibrator.getEstimatedMa());
26240 assertNull(calibrator.getEstimatedSx());
26241 assertNull(calibrator.getEstimatedSy());
26242 assertNull(calibrator.getEstimatedSz());
26243 assertNull(calibrator.getEstimatedMxy());
26244 assertNull(calibrator.getEstimatedMxz());
26245 assertNull(calibrator.getEstimatedMyx());
26246 assertNull(calibrator.getEstimatedMyz());
26247 assertNull(calibrator.getEstimatedMzx());
26248 assertNull(calibrator.getEstimatedMzy());
26249 assertNull(calibrator.getEstimatedCovariance());
26250 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
26251 assertNotNull(calibrator.getGroundTruthGravityNorm());
26252 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26253 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26254 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26255 ABSOLUTE_ERROR));
26256 final var g = new Acceleration(0.0, AccelerationUnit.G);
26257 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26258 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26259 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26260
26261
26262 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26263
26264 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26265 invalidGravityNorm, measurements, ba));
26266 final var m1 = new Matrix(1, 1);
26267 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26268 gravityNorm, measurements, m1));
26269 final var m2 = new Matrix(1, 3);
26270 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26271 gravityNorm, measurements, m2));
26272 }
26273
26274 @Test
26275 void testConstructor222() throws WrongSizeException {
26276 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
26277
26278 final var ba = generateBa();
26279 final var bias = ba.getBuffer();
26280 final var biasX = ba.getElementAtIndex(0);
26281 final var biasY = ba.getElementAtIndex(1);
26282 final var biasZ = ba.getElementAtIndex(2);
26283
26284 final var randomizer = new UniformRandomizer();
26285 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26286 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26287 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26288 final var nedPosition = new NEDPosition(latitude, longitude, height);
26289 final var nedVelocity = new NEDVelocity();
26290 final var ecefPosition = new ECEFPosition();
26291 final var ecefVelocity = new ECEFVelocity();
26292 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26293 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26294 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26295 final var gravityNorm = gravity.getNormAsAcceleration();
26296
26297 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba,
26298 this);
26299
26300
26301 assertEquals(biasX, calibrator.getBiasX(), 0.0);
26302 assertEquals(biasY, calibrator.getBiasY(), 0.0);
26303 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26304 final var bx1 = calibrator.getBiasXAsAcceleration();
26305 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26306 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26307 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26308 calibrator.getBiasXAsAcceleration(bx2);
26309 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26310 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26311 final var by1 = calibrator.getBiasYAsAcceleration();
26312 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26313 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26314 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26315 calibrator.getBiasYAsAcceleration(by2);
26316 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26317 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26318 final var bz1 = calibrator.getBiasZAsAcceleration();
26319 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26320 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26321 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26322 calibrator.getBiasZAsAcceleration(bz2);
26323 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26324 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26325 final var biasTriad1 = calibrator.getBiasAsTriad();
26326 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26327 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26328 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26329 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26330 final var biasTriad2 = new AccelerationTriad();
26331 calibrator.getBiasAsTriad(biasTriad2);
26332 assertEquals(biasTriad1, biasTriad2);
26333 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
26334 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
26335 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
26336 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
26337 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
26338 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
26339 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
26340 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
26341 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
26342 final var bias1 = calibrator.getBias();
26343 assertArrayEquals(bias1, bias, 0.0);
26344 final var bias2 = new double[3];
26345 calibrator.getBias(bias2);
26346 assertArrayEquals(bias1, bias2, 0.0);
26347 final var b1 = calibrator.getBiasAsMatrix();
26348 assertEquals(b1, ba);
26349 final var b2 = new Matrix(3, 1);
26350 calibrator.getBiasAsMatrix(b2);
26351 assertEquals(b1, b2);
26352 final var ma1 = calibrator.getInitialMa();
26353 assertEquals(new Matrix(3, 3), ma1);
26354 final var ma2 = new Matrix(3, 3);
26355 calibrator.getInitialMa(ma2);
26356 assertEquals(ma1, ma2);
26357 assertSame(measurements, calibrator.getMeasurements());
26358 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
26359 calibrator.getMeasurementType());
26360 assertFalse(calibrator.isOrderedMeasurementsRequired());
26361 assertFalse(calibrator.isQualityScoresRequired());
26362 assertFalse(calibrator.isCommonAxisUsed());
26363 assertSame(this, calibrator.getListener());
26364 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
26365 assertFalse(calibrator.isReady());
26366 assertFalse(calibrator.isRunning());
26367 assertNull(calibrator.getEstimatedMa());
26368 assertNull(calibrator.getEstimatedSx());
26369 assertNull(calibrator.getEstimatedSy());
26370 assertNull(calibrator.getEstimatedSz());
26371 assertNull(calibrator.getEstimatedMxy());
26372 assertNull(calibrator.getEstimatedMxz());
26373 assertNull(calibrator.getEstimatedMyx());
26374 assertNull(calibrator.getEstimatedMyz());
26375 assertNull(calibrator.getEstimatedMzx());
26376 assertNull(calibrator.getEstimatedMzy());
26377 assertNull(calibrator.getEstimatedCovariance());
26378 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
26379 assertNotNull(calibrator.getGroundTruthGravityNorm());
26380 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26381 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26382 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26383 ABSOLUTE_ERROR));
26384 final var g = new Acceleration(0.0, AccelerationUnit.G);
26385 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26386 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26387 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26388
26389
26390 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26391
26392 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26393 invalidGravityNorm, measurements, ba, this));
26394 final var m1 = new Matrix(1, 1);
26395 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26396 gravityNorm, measurements, m1, this));
26397 final var m2 = new Matrix(1, 3);
26398 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26399 gravityNorm, measurements, m2, this));
26400 }
26401
26402 @Test
26403 void testConstructor223() throws WrongSizeException {
26404 final var ba = generateBa();
26405 final var bias = ba.getBuffer();
26406 final var biasX = ba.getElementAtIndex(0);
26407 final var biasY = ba.getElementAtIndex(1);
26408 final var biasZ = ba.getElementAtIndex(2);
26409
26410 final var randomizer = new UniformRandomizer();
26411 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26412 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26413 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26414 final var nedPosition = new NEDPosition(latitude, longitude, height);
26415 final var nedVelocity = new NEDVelocity();
26416 final var ecefPosition = new ECEFPosition();
26417 final var ecefVelocity = new ECEFVelocity();
26418 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26419 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26420 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26421 final var gravityNorm = gravity.getNormAsAcceleration();
26422
26423 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, ba);
26424
26425
26426 assertEquals(biasX, calibrator.getBiasX(), 0.0);
26427 assertEquals(biasY, calibrator.getBiasY(), 0.0);
26428 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26429 final var bx1 = calibrator.getBiasXAsAcceleration();
26430 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26431 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26432 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26433 calibrator.getBiasXAsAcceleration(bx2);
26434 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26435 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26436 final var by1 = calibrator.getBiasYAsAcceleration();
26437 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26438 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26439 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26440 calibrator.getBiasYAsAcceleration(by2);
26441 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26442 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26443 final var bz1 = calibrator.getBiasZAsAcceleration();
26444 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26445 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26446 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26447 calibrator.getBiasZAsAcceleration(bz2);
26448 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26449 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26450 final var biasTriad1 = calibrator.getBiasAsTriad();
26451 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26452 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26453 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26454 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26455 final var biasTriad2 = new AccelerationTriad();
26456 calibrator.getBiasAsTriad(biasTriad2);
26457 assertEquals(biasTriad1, biasTriad2);
26458 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
26459 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
26460 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
26461 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
26462 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
26463 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
26464 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
26465 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
26466 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
26467 final var bias1 = calibrator.getBias();
26468 assertArrayEquals(bias1, bias, 0.0);
26469 final var bias2 = new double[3];
26470 calibrator.getBias(bias2);
26471 assertArrayEquals(bias1, bias2, 0.0);
26472 final var b1 = calibrator.getBiasAsMatrix();
26473 assertEquals(b1, ba);
26474 final var b2 = new Matrix(3, 1);
26475 calibrator.getBiasAsMatrix(b2);
26476 assertEquals(b1, b2);
26477 final var ma1 = calibrator.getInitialMa();
26478 assertEquals(new Matrix(3, 3), ma1);
26479 final var ma2 = new Matrix(3, 3);
26480 calibrator.getInitialMa(ma2);
26481 assertEquals(ma1, ma2);
26482 assertNull(calibrator.getMeasurements());
26483 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
26484 calibrator.getMeasurementType());
26485 assertFalse(calibrator.isOrderedMeasurementsRequired());
26486 assertFalse(calibrator.isQualityScoresRequired());
26487 assertTrue(calibrator.isCommonAxisUsed());
26488 assertNull(calibrator.getListener());
26489 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
26490 assertFalse(calibrator.isReady());
26491 assertFalse(calibrator.isRunning());
26492 assertNull(calibrator.getEstimatedMa());
26493 assertNull(calibrator.getEstimatedSx());
26494 assertNull(calibrator.getEstimatedSy());
26495 assertNull(calibrator.getEstimatedSz());
26496 assertNull(calibrator.getEstimatedMxy());
26497 assertNull(calibrator.getEstimatedMxz());
26498 assertNull(calibrator.getEstimatedMyx());
26499 assertNull(calibrator.getEstimatedMyz());
26500 assertNull(calibrator.getEstimatedMzx());
26501 assertNull(calibrator.getEstimatedMzy());
26502 assertNull(calibrator.getEstimatedCovariance());
26503 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
26504 assertNotNull(calibrator.getGroundTruthGravityNorm());
26505 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26506 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26507 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26508 ABSOLUTE_ERROR));
26509 final var g = new Acceleration(0.0, AccelerationUnit.G);
26510 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26511 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26512 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26513
26514
26515 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26516
26517 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26518 invalidGravityNorm, true, ba));
26519 final var m1 = new Matrix(1, 1);
26520 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26521 gravityNorm, true, m1));
26522 final var m2 = new Matrix(1, 3);
26523 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26524 gravityNorm, true, m2));
26525 }
26526
26527 @Test
26528 void testConstructor224() throws WrongSizeException {
26529 final var ba = generateBa();
26530 final var bias = ba.getBuffer();
26531 final var biasX = ba.getElementAtIndex(0);
26532 final var biasY = ba.getElementAtIndex(1);
26533 final var biasZ = ba.getElementAtIndex(2);
26534
26535 final var randomizer = new UniformRandomizer();
26536 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26537 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26538 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26539 final var nedPosition = new NEDPosition(latitude, longitude, height);
26540 final var nedVelocity = new NEDVelocity();
26541 final var ecefPosition = new ECEFPosition();
26542 final var ecefVelocity = new ECEFVelocity();
26543 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26544 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26545 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26546 final var gravityNorm = gravity.getNormAsAcceleration();
26547
26548 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, ba,
26549 this);
26550
26551
26552 assertEquals(biasX, calibrator.getBiasX(), 0.0);
26553 assertEquals(biasY, calibrator.getBiasY(), 0.0);
26554 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26555 final var bx1 = calibrator.getBiasXAsAcceleration();
26556 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26557 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26558 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26559 calibrator.getBiasXAsAcceleration(bx2);
26560 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26561 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26562 final var by1 = calibrator.getBiasYAsAcceleration();
26563 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26564 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26565 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26566 calibrator.getBiasYAsAcceleration(by2);
26567 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26568 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26569 final var bz1 = calibrator.getBiasZAsAcceleration();
26570 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26571 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26572 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26573 calibrator.getBiasZAsAcceleration(bz2);
26574 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26575 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26576 final var biasTriad1 = calibrator.getBiasAsTriad();
26577 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26578 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26579 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26580 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26581 final var biasTriad2 = new AccelerationTriad();
26582 calibrator.getBiasAsTriad(biasTriad2);
26583 assertEquals(biasTriad1, biasTriad2);
26584 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
26585 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
26586 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
26587 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
26588 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
26589 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
26590 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
26591 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
26592 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
26593 final var bias1 = calibrator.getBias();
26594 assertArrayEquals(bias1, bias, 0.0);
26595 final var bias2 = new double[3];
26596 calibrator.getBias(bias2);
26597 assertArrayEquals(bias1, bias2, 0.0);
26598 final var b1 = calibrator.getBiasAsMatrix();
26599 assertEquals(b1, ba);
26600 final var b2 = new Matrix(3, 1);
26601 calibrator.getBiasAsMatrix(b2);
26602 assertEquals(b1, b2);
26603 final var ma1 = calibrator.getInitialMa();
26604 assertEquals(new Matrix(3, 3), ma1);
26605 final var ma2 = new Matrix(3, 3);
26606 calibrator.getInitialMa(ma2);
26607 assertEquals(ma1, ma2);
26608 assertNull(calibrator.getMeasurements());
26609 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
26610 calibrator.getMeasurementType());
26611 assertFalse(calibrator.isOrderedMeasurementsRequired());
26612 assertFalse(calibrator.isQualityScoresRequired());
26613 assertTrue(calibrator.isCommonAxisUsed());
26614 assertSame(this, calibrator.getListener());
26615 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
26616 assertFalse(calibrator.isReady());
26617 assertFalse(calibrator.isRunning());
26618 assertNull(calibrator.getEstimatedMa());
26619 assertNull(calibrator.getEstimatedSx());
26620 assertNull(calibrator.getEstimatedSy());
26621 assertNull(calibrator.getEstimatedSz());
26622 assertNull(calibrator.getEstimatedMxy());
26623 assertNull(calibrator.getEstimatedMxz());
26624 assertNull(calibrator.getEstimatedMyx());
26625 assertNull(calibrator.getEstimatedMyz());
26626 assertNull(calibrator.getEstimatedMzx());
26627 assertNull(calibrator.getEstimatedMzy());
26628 assertNull(calibrator.getEstimatedCovariance());
26629 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
26630 assertNotNull(calibrator.getGroundTruthGravityNorm());
26631 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26632 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26633 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26634 ABSOLUTE_ERROR));
26635 final var g = new Acceleration(0.0, AccelerationUnit.G);
26636 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26637 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26638 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26639
26640
26641 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26642
26643 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26644 invalidGravityNorm, true, ba, this));
26645 final var m1 = new Matrix(1, 1);
26646 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26647 gravityNorm, true, m1, this));
26648 final var m2 = new Matrix(1, 3);
26649 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26650 gravityNorm, true, m2, this));
26651 }
26652
26653 @Test
26654 void testConstructor225() throws WrongSizeException {
26655 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
26656
26657 final var ba = generateBa();
26658 final var bias = ba.getBuffer();
26659 final var biasX = ba.getElementAtIndex(0);
26660 final var biasY = ba.getElementAtIndex(1);
26661 final var biasZ = ba.getElementAtIndex(2);
26662
26663 final var randomizer = new UniformRandomizer();
26664 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26665 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26666 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26667 final var nedPosition = new NEDPosition(latitude, longitude, height);
26668 final var nedVelocity = new NEDVelocity();
26669 final var ecefPosition = new ECEFPosition();
26670 final var ecefVelocity = new ECEFVelocity();
26671 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26672 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26673 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26674 final var gravityNorm = gravity.getNormAsAcceleration();
26675
26676 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
26677 true, ba);
26678
26679
26680 assertEquals(biasX, calibrator.getBiasX(), 0.0);
26681 assertEquals(biasY, calibrator.getBiasY(), 0.0);
26682 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26683 final var bx1 = calibrator.getBiasXAsAcceleration();
26684 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26685 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26686 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26687 calibrator.getBiasXAsAcceleration(bx2);
26688 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26689 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26690 final var by1 = calibrator.getBiasYAsAcceleration();
26691 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26692 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26693 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26694 calibrator.getBiasYAsAcceleration(by2);
26695 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26696 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26697 final var bz1 = calibrator.getBiasZAsAcceleration();
26698 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26699 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26700 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26701 calibrator.getBiasZAsAcceleration(bz2);
26702 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26703 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26704 final var biasTriad1 = calibrator.getBiasAsTriad();
26705 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26706 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26707 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26708 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26709 final var biasTriad2 = new AccelerationTriad();
26710 calibrator.getBiasAsTriad(biasTriad2);
26711 assertEquals(biasTriad1, biasTriad2);
26712 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
26713 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
26714 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
26715 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
26716 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
26717 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
26718 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
26719 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
26720 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
26721 final var bias1 = calibrator.getBias();
26722 assertArrayEquals(bias1, bias, 0.0);
26723 final var bias2 = new double[3];
26724 calibrator.getBias(bias2);
26725 assertArrayEquals(bias1, bias2, 0.0);
26726 final var b1 = calibrator.getBiasAsMatrix();
26727 assertEquals(b1, ba);
26728 final var b2 = new Matrix(3, 1);
26729 calibrator.getBiasAsMatrix(b2);
26730 assertEquals(b1, b2);
26731 final var ma1 = calibrator.getInitialMa();
26732 assertEquals(new Matrix(3, 3), ma1);
26733 final var ma2 = new Matrix(3, 3);
26734 calibrator.getInitialMa(ma2);
26735 assertEquals(ma1, ma2);
26736 assertSame(measurements, calibrator.getMeasurements());
26737 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
26738 calibrator.getMeasurementType());
26739 assertFalse(calibrator.isOrderedMeasurementsRequired());
26740 assertFalse(calibrator.isQualityScoresRequired());
26741 assertTrue(calibrator.isCommonAxisUsed());
26742 assertNull(calibrator.getListener());
26743 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
26744 assertFalse(calibrator.isReady());
26745 assertFalse(calibrator.isRunning());
26746 assertNull(calibrator.getEstimatedMa());
26747 assertNull(calibrator.getEstimatedSx());
26748 assertNull(calibrator.getEstimatedSy());
26749 assertNull(calibrator.getEstimatedSz());
26750 assertNull(calibrator.getEstimatedMxy());
26751 assertNull(calibrator.getEstimatedMxz());
26752 assertNull(calibrator.getEstimatedMyx());
26753 assertNull(calibrator.getEstimatedMyz());
26754 assertNull(calibrator.getEstimatedMzx());
26755 assertNull(calibrator.getEstimatedMzy());
26756 assertNull(calibrator.getEstimatedCovariance());
26757 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
26758 assertNotNull(calibrator.getGroundTruthGravityNorm());
26759 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26760 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26761 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26762 ABSOLUTE_ERROR));
26763 final var g = new Acceleration(0.0, AccelerationUnit.G);
26764 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26765 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26766 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26767
26768
26769 final var invalidGravityNorm = new Acceleration(
26770 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26771
26772 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26773 invalidGravityNorm, measurements, true, ba));
26774 final var m1 = new Matrix(1, 1);
26775 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26776 gravityNorm, measurements, true, m1));
26777 final var m2 = new Matrix(1, 3);
26778 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26779 gravityNorm, measurements, true, m2));
26780 }
26781
26782 @Test
26783 void testConstructor226() throws WrongSizeException {
26784 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
26785
26786 final var ba = generateBa();
26787 final var bias = ba.getBuffer();
26788 final var biasX = ba.getElementAtIndex(0);
26789 final var biasY = ba.getElementAtIndex(1);
26790 final var biasZ = ba.getElementAtIndex(2);
26791
26792 final var randomizer = new UniformRandomizer();
26793 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26794 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26795 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26796 final var nedPosition = new NEDPosition(latitude, longitude, height);
26797 final var nedVelocity = new NEDVelocity();
26798 final var ecefPosition = new ECEFPosition();
26799 final var ecefVelocity = new ECEFVelocity();
26800 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26801 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26802 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26803 final var gravityNorm = gravity.getNormAsAcceleration();
26804
26805 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
26806 true, ba, this);
26807
26808
26809 assertEquals(biasX, calibrator.getBiasX(), 0.0);
26810 assertEquals(biasY, calibrator.getBiasY(), 0.0);
26811 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26812 final var bx1 = calibrator.getBiasXAsAcceleration();
26813 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26814 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26815 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26816 calibrator.getBiasXAsAcceleration(bx2);
26817 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26818 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26819 final var by1 = calibrator.getBiasYAsAcceleration();
26820 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26821 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26822 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26823 calibrator.getBiasYAsAcceleration(by2);
26824 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26825 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26826 final var bz1 = calibrator.getBiasZAsAcceleration();
26827 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26828 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26829 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26830 calibrator.getBiasZAsAcceleration(bz2);
26831 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26832 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26833 final var biasTriad1 = calibrator.getBiasAsTriad();
26834 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26835 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26836 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26837 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26838 final var biasTriad2 = new AccelerationTriad();
26839 calibrator.getBiasAsTriad(biasTriad2);
26840 assertEquals(biasTriad1, biasTriad2);
26841 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
26842 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
26843 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
26844 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
26845 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
26846 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
26847 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
26848 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
26849 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
26850 final var bias1 = calibrator.getBias();
26851 assertArrayEquals(bias1, bias, 0.0);
26852 final var bias2 = new double[3];
26853 calibrator.getBias(bias2);
26854 assertArrayEquals(bias1, bias2, 0.0);
26855 final var b1 = calibrator.getBiasAsMatrix();
26856 assertEquals(b1, ba);
26857 final var b2 = new Matrix(3, 1);
26858 calibrator.getBiasAsMatrix(b2);
26859 assertEquals(b1, b2);
26860 final var ma1 = calibrator.getInitialMa();
26861 assertEquals(new Matrix(3, 3), ma1);
26862 final var ma2 = new Matrix(3, 3);
26863 calibrator.getInitialMa(ma2);
26864 assertEquals(ma1, ma2);
26865 assertSame(measurements, calibrator.getMeasurements());
26866 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
26867 calibrator.getMeasurementType());
26868 assertFalse(calibrator.isOrderedMeasurementsRequired());
26869 assertFalse(calibrator.isQualityScoresRequired());
26870 assertTrue(calibrator.isCommonAxisUsed());
26871 assertSame(this, calibrator.getListener());
26872 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
26873 assertFalse(calibrator.isReady());
26874 assertFalse(calibrator.isRunning());
26875 assertNull(calibrator.getEstimatedMa());
26876 assertNull(calibrator.getEstimatedSx());
26877 assertNull(calibrator.getEstimatedSy());
26878 assertNull(calibrator.getEstimatedSz());
26879 assertNull(calibrator.getEstimatedMxy());
26880 assertNull(calibrator.getEstimatedMxz());
26881 assertNull(calibrator.getEstimatedMyx());
26882 assertNull(calibrator.getEstimatedMyz());
26883 assertNull(calibrator.getEstimatedMzx());
26884 assertNull(calibrator.getEstimatedMzy());
26885 assertNull(calibrator.getEstimatedCovariance());
26886 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
26887 assertNotNull(calibrator.getGroundTruthGravityNorm());
26888 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26889 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26890 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26891 ABSOLUTE_ERROR));
26892 final var g = new Acceleration(0.0, AccelerationUnit.G);
26893 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26894 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26895 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26896
26897
26898 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26899
26900 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26901 invalidGravityNorm, measurements, true, ba, this));
26902 final var m1 = new Matrix(1, 1);
26903 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26904 gravityNorm, measurements, true, m1, this));
26905 final var m2 = new Matrix(1, 3);
26906 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26907 gravityNorm, measurements, true, m2, this));
26908 }
26909
26910 @Test
26911 void testConstructor227() throws WrongSizeException {
26912 final var ba = generateBa();
26913 final var bias = ba.getBuffer();
26914 final var biasX = ba.getElementAtIndex(0);
26915 final var biasY = ba.getElementAtIndex(1);
26916 final var biasZ = ba.getElementAtIndex(2);
26917
26918 final var ma = generateMaCommonAxis();
26919 final var sx = ma.getElementAt(0, 0);
26920 final var sy = ma.getElementAt(1, 1);
26921 final var sz = ma.getElementAt(2, 2);
26922 final var mxy = ma.getElementAt(0, 1);
26923 final var mxz = ma.getElementAt(0, 2);
26924 final var myx = ma.getElementAt(1, 0);
26925 final var myz = ma.getElementAt(1, 2);
26926 final var mzx = ma.getElementAt(2, 0);
26927 final var mzy = ma.getElementAt(2, 1);
26928
26929 final var randomizer = new UniformRandomizer();
26930 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26931 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26932 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26933 final var nedPosition = new NEDPosition(latitude, longitude, height);
26934 final var nedVelocity = new NEDVelocity();
26935 final var ecefPosition = new ECEFPosition();
26936 final var ecefVelocity = new ECEFVelocity();
26937 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26938 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26939 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26940 final var gravityNorm = gravity.getNormAsAcceleration();
26941
26942 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba, ma);
26943
26944
26945 assertEquals(biasX, calibrator.getBiasX(), 0.0);
26946 assertEquals(biasY, calibrator.getBiasY(), 0.0);
26947 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26948 final var bx1 = calibrator.getBiasXAsAcceleration();
26949 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26950 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26951 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26952 calibrator.getBiasXAsAcceleration(bx2);
26953 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26954 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26955 final var by1 = calibrator.getBiasYAsAcceleration();
26956 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26957 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26958 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26959 calibrator.getBiasYAsAcceleration(by2);
26960 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26961 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26962 final var bz1 = calibrator.getBiasZAsAcceleration();
26963 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26964 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26965 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26966 calibrator.getBiasZAsAcceleration(bz2);
26967 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26968 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26969 final var biasTriad1 = calibrator.getBiasAsTriad();
26970 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26971 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26972 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26973 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26974 final var biasTriad2 = new AccelerationTriad();
26975 calibrator.getBiasAsTriad(biasTriad2);
26976 assertEquals(biasTriad1, biasTriad2);
26977 assertEquals(sx, calibrator.getInitialSx(), 0.0);
26978 assertEquals(sy, calibrator.getInitialSy(), 0.0);
26979 assertEquals(sz, calibrator.getInitialSz(), 0.0);
26980 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
26981 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
26982 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
26983 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
26984 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
26985 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
26986 final var bias1 = calibrator.getBias();
26987 assertArrayEquals(bias1, bias, 0.0);
26988 final var bias2 = new double[3];
26989 calibrator.getBias(bias2);
26990 assertArrayEquals(bias1, bias2, 0.0);
26991 final var b1 = calibrator.getBiasAsMatrix();
26992 assertEquals(b1, ba);
26993 final var b2 = new Matrix(3, 1);
26994 calibrator.getBiasAsMatrix(b2);
26995 assertEquals(b1, b2);
26996 final var ma1 = new Matrix(3, 3);
26997 ma1.setSubmatrix(0, 0, 2, 2,
26998 new double[]{sx, myx, mzx,
26999 mxy, sy, mzy,
27000 mxz, myz, sz});
27001 assertEquals(ma1, calibrator.getInitialMa());
27002 final var ma2 = new Matrix(3, 3);
27003 calibrator.getInitialMa(ma2);
27004 assertEquals(ma1, ma2);
27005 assertNull(calibrator.getMeasurements());
27006 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
27007 calibrator.getMeasurementType());
27008 assertFalse(calibrator.isOrderedMeasurementsRequired());
27009 assertFalse(calibrator.isQualityScoresRequired());
27010 assertFalse(calibrator.isCommonAxisUsed());
27011 assertNull(calibrator.getListener());
27012 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
27013 assertFalse(calibrator.isReady());
27014 assertFalse(calibrator.isRunning());
27015 assertNull(calibrator.getEstimatedMa());
27016 assertNull(calibrator.getEstimatedSx());
27017 assertNull(calibrator.getEstimatedSy());
27018 assertNull(calibrator.getEstimatedSz());
27019 assertNull(calibrator.getEstimatedMxy());
27020 assertNull(calibrator.getEstimatedMxz());
27021 assertNull(calibrator.getEstimatedMyx());
27022 assertNull(calibrator.getEstimatedMyz());
27023 assertNull(calibrator.getEstimatedMzx());
27024 assertNull(calibrator.getEstimatedMzy());
27025 assertNull(calibrator.getEstimatedCovariance());
27026 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
27027 assertNotNull(calibrator.getGroundTruthGravityNorm());
27028 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27029 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27030 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
27031 ABSOLUTE_ERROR));
27032 final var g = new Acceleration(0.0, AccelerationUnit.G);
27033 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27034 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27035 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
27036
27037
27038 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27039
27040 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27041 invalidGravityNorm, ba, ma));
27042 final var m1 = new Matrix(1, 1);
27043 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27044 gravityNorm, m1, ma));
27045 final var m2 = new Matrix(1, 3);
27046 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27047 gravityNorm, m2, ma));
27048 final var m3 = new Matrix(1, 3);
27049 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27050 gravityNorm, ba, m3));
27051 final var m4 = new Matrix(3, 1);
27052 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27053 gravityNorm, ba, m4));
27054 }
27055
27056 @Test
27057 void testConstructor228() throws WrongSizeException {
27058 final var ba = generateBa();
27059 final var bias = ba.getBuffer();
27060 final var biasX = ba.getElementAtIndex(0);
27061 final var biasY = ba.getElementAtIndex(1);
27062 final var biasZ = ba.getElementAtIndex(2);
27063
27064 final var ma = generateMaCommonAxis();
27065 final var sx = ma.getElementAt(0, 0);
27066 final var sy = ma.getElementAt(1, 1);
27067 final var sz = ma.getElementAt(2, 2);
27068 final var mxy = ma.getElementAt(0, 1);
27069 final var mxz = ma.getElementAt(0, 2);
27070 final var myx = ma.getElementAt(1, 0);
27071 final var myz = ma.getElementAt(1, 2);
27072 final var mzx = ma.getElementAt(2, 0);
27073 final var mzy = ma.getElementAt(2, 1);
27074
27075 final var randomizer = new UniformRandomizer();
27076 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27077 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27078 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27079 final var nedPosition = new NEDPosition(latitude, longitude, height);
27080 final var nedVelocity = new NEDVelocity();
27081 final var ecefPosition = new ECEFPosition();
27082 final var ecefVelocity = new ECEFVelocity();
27083 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
27084 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27085 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27086 final var gravityNorm = gravity.getNormAsAcceleration();
27087
27088 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba, ma, this);
27089
27090
27091 assertEquals(biasX, calibrator.getBiasX(), 0.0);
27092 assertEquals(biasY, calibrator.getBiasY(), 0.0);
27093 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
27094 final var bx1 = calibrator.getBiasXAsAcceleration();
27095 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
27096 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
27097 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27098 calibrator.getBiasXAsAcceleration(bx2);
27099 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
27100 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
27101 final var by1 = calibrator.getBiasYAsAcceleration();
27102 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
27103 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
27104 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27105 calibrator.getBiasYAsAcceleration(by2);
27106 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
27107 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
27108 final var bz1 = calibrator.getBiasZAsAcceleration();
27109 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
27110 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
27111 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27112 calibrator.getBiasZAsAcceleration(bz2);
27113 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
27114 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
27115 final var biasTriad1 = calibrator.getBiasAsTriad();
27116 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
27117 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
27118 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
27119 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
27120 final var biasTriad2 = new AccelerationTriad();
27121 calibrator.getBiasAsTriad(biasTriad2);
27122 assertEquals(biasTriad1, biasTriad2);
27123 assertEquals(sx, calibrator.getInitialSx(), 0.0);
27124 assertEquals(sy, calibrator.getInitialSy(), 0.0);
27125 assertEquals(sz, calibrator.getInitialSz(), 0.0);
27126 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
27127 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
27128 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
27129 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
27130 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
27131 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
27132 final var bias1 = calibrator.getBias();
27133 assertArrayEquals(bias1, bias, 0.0);
27134 final var bias2 = new double[3];
27135 calibrator.getBias(bias2);
27136 assertArrayEquals(bias1, bias2, 0.0);
27137 final var b1 = calibrator.getBiasAsMatrix();
27138 assertEquals(b1, ba);
27139 final var b2 = new Matrix(3, 1);
27140 calibrator.getBiasAsMatrix(b2);
27141 assertEquals(b1, b2);
27142 final var ma1 = new Matrix(3, 3);
27143 ma1.setSubmatrix(0, 0, 2, 2,
27144 new double[]{sx, myx, mzx,
27145 mxy, sy, mzy,
27146 mxz, myz, sz});
27147 assertEquals(ma1, calibrator.getInitialMa());
27148 final var ma2 = new Matrix(3, 3);
27149 calibrator.getInitialMa(ma2);
27150 assertEquals(ma1, ma2);
27151 assertNull(calibrator.getMeasurements());
27152 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
27153 calibrator.getMeasurementType());
27154 assertFalse(calibrator.isOrderedMeasurementsRequired());
27155 assertFalse(calibrator.isQualityScoresRequired());
27156 assertFalse(calibrator.isCommonAxisUsed());
27157 assertSame(this, calibrator.getListener());
27158 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
27159 assertFalse(calibrator.isReady());
27160 assertFalse(calibrator.isRunning());
27161 assertNull(calibrator.getEstimatedMa());
27162 assertNull(calibrator.getEstimatedSx());
27163 assertNull(calibrator.getEstimatedSy());
27164 assertNull(calibrator.getEstimatedSz());
27165 assertNull(calibrator.getEstimatedMxy());
27166 assertNull(calibrator.getEstimatedMxz());
27167 assertNull(calibrator.getEstimatedMyx());
27168 assertNull(calibrator.getEstimatedMyz());
27169 assertNull(calibrator.getEstimatedMzx());
27170 assertNull(calibrator.getEstimatedMzy());
27171 assertNull(calibrator.getEstimatedCovariance());
27172 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
27173 assertNotNull(calibrator.getGroundTruthGravityNorm());
27174 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27175 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27176 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
27177 ABSOLUTE_ERROR));
27178 final var g = new Acceleration(0.0, AccelerationUnit.G);
27179 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27180 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27181 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
27182
27183
27184 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27185
27186 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27187 invalidGravityNorm, ba, ma, this));
27188 final var m1 = new Matrix(1, 1);
27189 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27190 gravityNorm, m1, ma, this));
27191 final var m2 = new Matrix(1, 3);
27192 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27193 gravityNorm, m2, ma, this));
27194 final var m3 = new Matrix(1, 3);
27195 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27196 gravityNorm, ba, m3, this));
27197 final var m4 = new Matrix(3, 1);
27198 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27199 gravityNorm, ba, m4, this));
27200 }
27201
27202 @Test
27203 void testConstructor229() throws WrongSizeException {
27204 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
27205
27206 final var ba = generateBa();
27207 final var bias = ba.getBuffer();
27208 final var biasX = ba.getElementAtIndex(0);
27209 final var biasY = ba.getElementAtIndex(1);
27210 final var biasZ = ba.getElementAtIndex(2);
27211
27212 final var ma = generateMaCommonAxis();
27213 final var sx = ma.getElementAt(0, 0);
27214 final var sy = ma.getElementAt(1, 1);
27215 final var sz = ma.getElementAt(2, 2);
27216 final var mxy = ma.getElementAt(0, 1);
27217 final var mxz = ma.getElementAt(0, 2);
27218 final var myx = ma.getElementAt(1, 0);
27219 final var myz = ma.getElementAt(1, 2);
27220 final var mzx = ma.getElementAt(2, 0);
27221 final var mzy = ma.getElementAt(2, 1);
27222
27223 final var randomizer = new UniformRandomizer();
27224 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27225 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27226 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27227 final var nedPosition = new NEDPosition(latitude, longitude, height);
27228 final var nedVelocity = new NEDVelocity();
27229 final var ecefPosition = new ECEFPosition();
27230 final var ecefVelocity = new ECEFVelocity();
27231 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
27232 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27233 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27234 final var gravityNorm = gravity.getNormAsAcceleration();
27235
27236 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba, ma);
27237
27238
27239 assertEquals(biasX, calibrator.getBiasX(), 0.0);
27240 assertEquals(biasY, calibrator.getBiasY(), 0.0);
27241 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
27242 final var bx1 = calibrator.getBiasXAsAcceleration();
27243 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
27244 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
27245 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27246 calibrator.getBiasXAsAcceleration(bx2);
27247 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
27248 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
27249 final var by1 = calibrator.getBiasYAsAcceleration();
27250 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
27251 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
27252 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27253 calibrator.getBiasYAsAcceleration(by2);
27254 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
27255 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
27256 final var bz1 = calibrator.getBiasZAsAcceleration();
27257 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
27258 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
27259 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27260 calibrator.getBiasZAsAcceleration(bz2);
27261 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
27262 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
27263 final var biasTriad1 = calibrator.getBiasAsTriad();
27264 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
27265 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
27266 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
27267 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
27268 final var biasTriad2 = new AccelerationTriad();
27269 calibrator.getBiasAsTriad(biasTriad2);
27270 assertEquals(biasTriad1, biasTriad2);
27271 assertEquals(sx, calibrator.getInitialSx(), 0.0);
27272 assertEquals(sy, calibrator.getInitialSy(), 0.0);
27273 assertEquals(sz, calibrator.getInitialSz(), 0.0);
27274 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
27275 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
27276 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
27277 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
27278 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
27279 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
27280 final var bias1 = calibrator.getBias();
27281 assertArrayEquals(bias1, bias, 0.0);
27282 final var bias2 = new double[3];
27283 calibrator.getBias(bias2);
27284 assertArrayEquals(bias1, bias2, 0.0);
27285 final var b1 = calibrator.getBiasAsMatrix();
27286 assertEquals(b1, ba);
27287 final var b2 = new Matrix(3, 1);
27288 calibrator.getBiasAsMatrix(b2);
27289 assertEquals(b1, b2);
27290 final var ma1 = new Matrix(3, 3);
27291 ma1.setSubmatrix(0, 0, 2, 2,
27292 new double[]{sx, myx, mzx,
27293 mxy, sy, mzy,
27294 mxz, myz, sz});
27295 assertEquals(ma1, calibrator.getInitialMa());
27296 final var ma2 = new Matrix(3, 3);
27297 calibrator.getInitialMa(ma2);
27298 assertEquals(ma1, ma2);
27299 assertSame(measurements, calibrator.getMeasurements());
27300 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
27301 calibrator.getMeasurementType());
27302 assertFalse(calibrator.isOrderedMeasurementsRequired());
27303 assertFalse(calibrator.isQualityScoresRequired());
27304 assertFalse(calibrator.isCommonAxisUsed());
27305 assertNull(calibrator.getListener());
27306 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
27307 assertFalse(calibrator.isReady());
27308 assertFalse(calibrator.isRunning());
27309 assertNull(calibrator.getEstimatedMa());
27310 assertNull(calibrator.getEstimatedSx());
27311 assertNull(calibrator.getEstimatedSy());
27312 assertNull(calibrator.getEstimatedSz());
27313 assertNull(calibrator.getEstimatedMxy());
27314 assertNull(calibrator.getEstimatedMxz());
27315 assertNull(calibrator.getEstimatedMyx());
27316 assertNull(calibrator.getEstimatedMyz());
27317 assertNull(calibrator.getEstimatedMzx());
27318 assertNull(calibrator.getEstimatedMzy());
27319 assertNull(calibrator.getEstimatedCovariance());
27320 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
27321 assertNotNull(calibrator.getGroundTruthGravityNorm());
27322 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27323 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27324 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
27325 ABSOLUTE_ERROR));
27326 final var g = new Acceleration(0.0, AccelerationUnit.G);
27327 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27328 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27329 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
27330
27331
27332 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27333
27334 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27335 invalidGravityNorm, ba, ma, this));
27336 final var m1 = new Matrix(1, 1);
27337 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27338 gravityNorm, measurements, m1, ma));
27339 final var m2 = new Matrix(1, 3);
27340 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27341 gravityNorm, measurements, m2, ma));
27342 final var m3 = new Matrix(1, 3);
27343 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27344 gravityNorm, measurements, ba, m3));
27345 final var m4 = new Matrix(3, 1);
27346 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27347 gravityNorm, measurements, ba, m4));
27348 }
27349
27350 @Test
27351 void testConstructor230() throws WrongSizeException {
27352 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
27353
27354 final var ba = generateBa();
27355 final var bias = ba.getBuffer();
27356 final var biasX = ba.getElementAtIndex(0);
27357 final var biasY = ba.getElementAtIndex(1);
27358 final var biasZ = ba.getElementAtIndex(2);
27359
27360 final var ma = generateMaCommonAxis();
27361 final var sx = ma.getElementAt(0, 0);
27362 final var sy = ma.getElementAt(1, 1);
27363 final var sz = ma.getElementAt(2, 2);
27364 final var mxy = ma.getElementAt(0, 1);
27365 final var mxz = ma.getElementAt(0, 2);
27366 final var myx = ma.getElementAt(1, 0);
27367 final var myz = ma.getElementAt(1, 2);
27368 final var mzx = ma.getElementAt(2, 0);
27369 final var mzy = ma.getElementAt(2, 1);
27370
27371 final var randomizer = new UniformRandomizer();
27372 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27373 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27374 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27375 final var nedPosition = new NEDPosition(latitude, longitude, height);
27376 final var nedVelocity = new NEDVelocity();
27377 final var ecefPosition = new ECEFPosition();
27378 final var ecefVelocity = new ECEFVelocity();
27379 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
27380 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27381 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27382 final var gravityNorm = gravity.getNormAsAcceleration();
27383
27384 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba, ma,
27385 this);
27386
27387
27388 assertEquals(biasX, calibrator.getBiasX(), 0.0);
27389 assertEquals(biasY, calibrator.getBiasY(), 0.0);
27390 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
27391 final var bx1 = calibrator.getBiasXAsAcceleration();
27392 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
27393 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
27394 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27395 calibrator.getBiasXAsAcceleration(bx2);
27396 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
27397 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
27398 final var by1 = calibrator.getBiasYAsAcceleration();
27399 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
27400 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
27401 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27402 calibrator.getBiasYAsAcceleration(by2);
27403 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
27404 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
27405 final var bz1 = calibrator.getBiasZAsAcceleration();
27406 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
27407 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
27408 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27409 calibrator.getBiasZAsAcceleration(bz2);
27410 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
27411 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
27412 final var biasTriad1 = calibrator.getBiasAsTriad();
27413 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
27414 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
27415 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
27416 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
27417 final var biasTriad2 = new AccelerationTriad();
27418 calibrator.getBiasAsTriad(biasTriad2);
27419 assertEquals(biasTriad1, biasTriad2);
27420 assertEquals(sx, calibrator.getInitialSx(), 0.0);
27421 assertEquals(sy, calibrator.getInitialSy(), 0.0);
27422 assertEquals(sz, calibrator.getInitialSz(), 0.0);
27423 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
27424 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
27425 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
27426 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
27427 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
27428 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
27429 final var bias1 = calibrator.getBias();
27430 assertArrayEquals(bias1, bias, 0.0);
27431 final var bias2 = new double[3];
27432 calibrator.getBias(bias2);
27433 assertArrayEquals(bias1, bias2, 0.0);
27434 final var b1 = calibrator.getBiasAsMatrix();
27435 assertEquals(b1, ba);
27436 final var b2 = new Matrix(3, 1);
27437 calibrator.getBiasAsMatrix(b2);
27438 assertEquals(b1, b2);
27439 final var ma1 = new Matrix(3, 3);
27440 ma1.setSubmatrix(0, 0, 2, 2,
27441 new double[]{sx, myx, mzx,
27442 mxy, sy, mzy,
27443 mxz, myz, sz});
27444 assertEquals(calibrator.getInitialMa(), ma1);
27445 final var ma2 = new Matrix(3, 3);
27446 calibrator.getInitialMa(ma2);
27447 assertEquals(ma1, ma2);
27448 assertSame(measurements, calibrator.getMeasurements());
27449 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
27450 calibrator.getMeasurementType());
27451 assertFalse(calibrator.isOrderedMeasurementsRequired());
27452 assertFalse(calibrator.isQualityScoresRequired());
27453 assertFalse(calibrator.isCommonAxisUsed());
27454 assertSame(this, calibrator.getListener());
27455 assertEquals(10, calibrator.getMinimumRequiredMeasurements());
27456 assertFalse(calibrator.isReady());
27457 assertFalse(calibrator.isRunning());
27458 assertNull(calibrator.getEstimatedMa());
27459 assertNull(calibrator.getEstimatedSx());
27460 assertNull(calibrator.getEstimatedSy());
27461 assertNull(calibrator.getEstimatedSz());
27462 assertNull(calibrator.getEstimatedMxy());
27463 assertNull(calibrator.getEstimatedMxz());
27464 assertNull(calibrator.getEstimatedMyx());
27465 assertNull(calibrator.getEstimatedMyz());
27466 assertNull(calibrator.getEstimatedMzx());
27467 assertNull(calibrator.getEstimatedMzy());
27468 assertNull(calibrator.getEstimatedCovariance());
27469 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
27470 assertNotNull(calibrator.getGroundTruthGravityNorm());
27471 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27472 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27473 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27474 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27475 final var g = new Acceleration(0.0, AccelerationUnit.G);
27476 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27477 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27478 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
27479
27480
27481 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27482
27483 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27484 invalidGravityNorm, ba, ma, this));
27485 final var m1 = new Matrix(1, 1);
27486 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27487 gravityNorm, measurements, m1, ma, this));
27488 final var m2 = new Matrix(1, 3);
27489 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27490 gravityNorm, measurements, m2, ma, this));
27491 final var m3 = new Matrix(1, 3);
27492 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27493 gravityNorm, measurements, ba, m3, this));
27494 final var m4 = new Matrix(3, 1);
27495 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27496 gravityNorm, measurements, ba, m4, this));
27497 }
27498
27499 @Test
27500 void testConstructor231() throws WrongSizeException {
27501 final var ba = generateBa();
27502 final var bias = ba.getBuffer();
27503 final var biasX = ba.getElementAtIndex(0);
27504 final var biasY = ba.getElementAtIndex(1);
27505 final var biasZ = ba.getElementAtIndex(2);
27506
27507 final var ma = generateMaCommonAxis();
27508 final var sx = ma.getElementAt(0, 0);
27509 final var sy = ma.getElementAt(1, 1);
27510 final var sz = ma.getElementAt(2, 2);
27511 final var mxy = ma.getElementAt(0, 1);
27512 final var mxz = ma.getElementAt(0, 2);
27513 final var myx = ma.getElementAt(1, 0);
27514 final var myz = ma.getElementAt(1, 2);
27515 final var mzx = ma.getElementAt(2, 0);
27516 final var mzy = ma.getElementAt(2, 1);
27517
27518 final var randomizer = new UniformRandomizer();
27519 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27520 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27521 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27522 final var nedPosition = new NEDPosition(latitude, longitude, height);
27523 final var nedVelocity = new NEDVelocity();
27524 final var ecefPosition = new ECEFPosition();
27525 final var ecefVelocity = new ECEFVelocity();
27526 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
27527 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27528 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27529 final var gravityNorm = gravity.getNormAsAcceleration();
27530
27531 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, ba,
27532 ma);
27533
27534
27535 assertEquals(biasX, calibrator.getBiasX(), 0.0);
27536 assertEquals(biasY, calibrator.getBiasY(), 0.0);
27537 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
27538 final var bx1 = calibrator.getBiasXAsAcceleration();
27539 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
27540 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
27541 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27542 calibrator.getBiasXAsAcceleration(bx2);
27543 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
27544 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
27545 final var by1 = calibrator.getBiasYAsAcceleration();
27546 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
27547 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
27548 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27549 calibrator.getBiasYAsAcceleration(by2);
27550 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
27551 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
27552 final var bz1 = calibrator.getBiasZAsAcceleration();
27553 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
27554 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
27555 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27556 calibrator.getBiasZAsAcceleration(bz2);
27557 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
27558 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
27559 final var biasTriad1 = calibrator.getBiasAsTriad();
27560 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
27561 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
27562 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
27563 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
27564 final var biasTriad2 = new AccelerationTriad();
27565 calibrator.getBiasAsTriad(biasTriad2);
27566 assertEquals(biasTriad1, biasTriad2);
27567 assertEquals(sx, calibrator.getInitialSx(), 0.0);
27568 assertEquals(sy, calibrator.getInitialSy(), 0.0);
27569 assertEquals(sz, calibrator.getInitialSz(), 0.0);
27570 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
27571 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
27572 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
27573 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
27574 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
27575 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
27576 final var bias1 = calibrator.getBias();
27577 assertArrayEquals(bias1, bias, 0.0);
27578 final var bias2 = new double[3];
27579 calibrator.getBias(bias2);
27580 assertArrayEquals(bias1, bias2, 0.0);
27581 final var b1 = calibrator.getBiasAsMatrix();
27582 assertEquals(b1, ba);
27583 final var b2 = new Matrix(3, 1);
27584 calibrator.getBiasAsMatrix(b2);
27585 assertEquals(b1, b2);
27586 final var ma1 = new Matrix(3, 3);
27587 ma1.setSubmatrix(0, 0, 2, 2,
27588 new double[]{sx, myx, mzx,
27589 mxy, sy, mzy,
27590 mxz, myz, sz});
27591 assertEquals(ma1, calibrator.getInitialMa());
27592 final var ma2 = new Matrix(3, 3);
27593 calibrator.getInitialMa(ma2);
27594 assertEquals(ma1, ma2);
27595 assertNull(calibrator.getMeasurements());
27596 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
27597 calibrator.getMeasurementType());
27598 assertFalse(calibrator.isOrderedMeasurementsRequired());
27599 assertFalse(calibrator.isQualityScoresRequired());
27600 assertTrue(calibrator.isCommonAxisUsed());
27601 assertNull(calibrator.getListener());
27602 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
27603 assertFalse(calibrator.isReady());
27604 assertFalse(calibrator.isRunning());
27605 assertNull(calibrator.getEstimatedMa());
27606 assertNull(calibrator.getEstimatedSx());
27607 assertNull(calibrator.getEstimatedSy());
27608 assertNull(calibrator.getEstimatedSz());
27609 assertNull(calibrator.getEstimatedMxy());
27610 assertNull(calibrator.getEstimatedMxz());
27611 assertNull(calibrator.getEstimatedMyx());
27612 assertNull(calibrator.getEstimatedMyz());
27613 assertNull(calibrator.getEstimatedMzx());
27614 assertNull(calibrator.getEstimatedMzy());
27615 assertNull(calibrator.getEstimatedCovariance());
27616 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
27617 assertNotNull(calibrator.getGroundTruthGravityNorm());
27618 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27619 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27620 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
27621 ABSOLUTE_ERROR));
27622 final var g = new Acceleration(0.0, AccelerationUnit.G);
27623 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27624 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27625 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
27626
27627
27628 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27629
27630 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27631 invalidGravityNorm, true, ba, ma));
27632 final var m1 = new Matrix(1, 1);
27633 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27634 gravityNorm, true, m1, ma));
27635 final var m2 = new Matrix(1, 3);
27636 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27637 gravityNorm, true, m2, ma));
27638 final var m3 = new Matrix(1, 3);
27639 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27640 gravityNorm, true, ba, m3));
27641 final var m4 = new Matrix(3, 1);
27642 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27643 gravityNorm, true, ba, m4));
27644 }
27645
27646 @Test
27647 void testConstructor232() throws WrongSizeException {
27648 final var ba = generateBa();
27649 final var bias = ba.getBuffer();
27650 final var biasX = ba.getElementAtIndex(0);
27651 final var biasY = ba.getElementAtIndex(1);
27652 final var biasZ = ba.getElementAtIndex(2);
27653
27654 final var ma = generateMaCommonAxis();
27655 final var sx = ma.getElementAt(0, 0);
27656 final var sy = ma.getElementAt(1, 1);
27657 final var sz = ma.getElementAt(2, 2);
27658 final var mxy = ma.getElementAt(0, 1);
27659 final var mxz = ma.getElementAt(0, 2);
27660 final var myx = ma.getElementAt(1, 0);
27661 final var myz = ma.getElementAt(1, 2);
27662 final var mzx = ma.getElementAt(2, 0);
27663 final var mzy = ma.getElementAt(2, 1);
27664
27665 final var randomizer = new UniformRandomizer();
27666 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27667 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27668 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27669 final var nedPosition = new NEDPosition(latitude, longitude, height);
27670 final var nedVelocity = new NEDVelocity();
27671 final var ecefPosition = new ECEFPosition();
27672 final var ecefVelocity = new ECEFVelocity();
27673 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
27674 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27675 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27676 final var gravityNorm = gravity.getNormAsAcceleration();
27677
27678 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, ba,
27679 ma, this);
27680
27681
27682 assertEquals(biasX, calibrator.getBiasX(), 0.0);
27683 assertEquals(biasY, calibrator.getBiasY(), 0.0);
27684 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
27685 final var bx1 = calibrator.getBiasXAsAcceleration();
27686 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
27687 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
27688 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27689 calibrator.getBiasXAsAcceleration(bx2);
27690 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
27691 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
27692 final var by1 = calibrator.getBiasYAsAcceleration();
27693 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
27694 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
27695 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27696 calibrator.getBiasYAsAcceleration(by2);
27697 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
27698 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
27699 final var bz1 = calibrator.getBiasZAsAcceleration();
27700 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
27701 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
27702 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27703 calibrator.getBiasZAsAcceleration(bz2);
27704 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
27705 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
27706 final var biasTriad1 = calibrator.getBiasAsTriad();
27707 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
27708 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
27709 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
27710 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
27711 final var biasTriad2 = new AccelerationTriad();
27712 calibrator.getBiasAsTriad(biasTriad2);
27713 assertEquals(biasTriad1, biasTriad2);
27714 assertEquals(sx, calibrator.getInitialSx(), 0.0);
27715 assertEquals(sy, calibrator.getInitialSy(), 0.0);
27716 assertEquals(sz, calibrator.getInitialSz(), 0.0);
27717 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
27718 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
27719 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
27720 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
27721 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
27722 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
27723 final var bias1 = calibrator.getBias();
27724 assertArrayEquals(bias1, bias, 0.0);
27725 final var bias2 = new double[3];
27726 calibrator.getBias(bias2);
27727 assertArrayEquals(bias1, bias2, 0.0);
27728 final var b1 = calibrator.getBiasAsMatrix();
27729 assertEquals(b1, ba);
27730 final var b2 = new Matrix(3, 1);
27731 calibrator.getBiasAsMatrix(b2);
27732 assertEquals(b1, b2);
27733 final var ma1 = new Matrix(3, 3);
27734 ma1.setSubmatrix(0, 0, 2, 2,
27735 new double[]{sx, myx, mzx,
27736 mxy, sy, mzy,
27737 mxz, myz, sz});
27738 assertEquals(ma1, calibrator.getInitialMa());
27739 final var ma2 = new Matrix(3, 3);
27740 calibrator.getInitialMa(ma2);
27741 assertEquals(ma1, ma2);
27742 assertNull(calibrator.getMeasurements());
27743 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
27744 calibrator.getMeasurementType());
27745 assertFalse(calibrator.isOrderedMeasurementsRequired());
27746 assertFalse(calibrator.isQualityScoresRequired());
27747 assertTrue(calibrator.isCommonAxisUsed());
27748 assertSame(this, calibrator.getListener());
27749 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
27750 assertFalse(calibrator.isReady());
27751 assertFalse(calibrator.isRunning());
27752 assertNull(calibrator.getEstimatedMa());
27753 assertNull(calibrator.getEstimatedSx());
27754 assertNull(calibrator.getEstimatedSy());
27755 assertNull(calibrator.getEstimatedSz());
27756 assertNull(calibrator.getEstimatedMxy());
27757 assertNull(calibrator.getEstimatedMxz());
27758 assertNull(calibrator.getEstimatedMyx());
27759 assertNull(calibrator.getEstimatedMyz());
27760 assertNull(calibrator.getEstimatedMzx());
27761 assertNull(calibrator.getEstimatedMzy());
27762 assertNull(calibrator.getEstimatedCovariance());
27763 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
27764 assertNotNull(calibrator.getGroundTruthGravityNorm());
27765 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27766 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27767 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
27768 ABSOLUTE_ERROR));
27769 final var g = new Acceleration(0.0, AccelerationUnit.G);
27770 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27771 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27772 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
27773
27774
27775 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27776
27777 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27778 invalidGravityNorm, true, ba, ma));
27779 final var m1 = new Matrix(1, 1);
27780 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27781 gravityNorm, true, m1, ma, this));
27782 final var m2 = new Matrix(1, 3);
27783 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27784 gravityNorm, true, m2, ma, this));
27785 final var m3 = new Matrix(1, 3);
27786 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27787 gravityNorm, true, ba, m3, this));
27788 final var m4 = new Matrix(3, 1);
27789 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27790 gravityNorm, true, ba, m4, this));
27791 }
27792
27793 @Test
27794 void testConstructor233() throws WrongSizeException {
27795 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
27796
27797 final var ba = generateBa();
27798 final var bias = ba.getBuffer();
27799 final var biasX = ba.getElementAtIndex(0);
27800 final var biasY = ba.getElementAtIndex(1);
27801 final var biasZ = ba.getElementAtIndex(2);
27802
27803 final var ma = generateMaCommonAxis();
27804 final var sx = ma.getElementAt(0, 0);
27805 final var sy = ma.getElementAt(1, 1);
27806 final var sz = ma.getElementAt(2, 2);
27807 final var mxy = ma.getElementAt(0, 1);
27808 final var mxz = ma.getElementAt(0, 2);
27809 final var myx = ma.getElementAt(1, 0);
27810 final var myz = ma.getElementAt(1, 2);
27811 final var mzx = ma.getElementAt(2, 0);
27812 final var mzy = ma.getElementAt(2, 1);
27813
27814 final var randomizer = new UniformRandomizer();
27815 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27816 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27817 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27818 final var nedPosition = new NEDPosition(latitude, longitude, height);
27819 final var nedVelocity = new NEDVelocity();
27820 final var ecefPosition = new ECEFPosition();
27821 final var ecefVelocity = new ECEFVelocity();
27822 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
27823 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27824 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27825 final var gravityNorm = gravity.getNormAsAcceleration();
27826
27827 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
27828 true, ba, ma);
27829
27830
27831 assertEquals(biasX, calibrator.getBiasX(), 0.0);
27832 assertEquals(biasY, calibrator.getBiasY(), 0.0);
27833 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
27834 final var bx1 = calibrator.getBiasXAsAcceleration();
27835 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
27836 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
27837 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27838 calibrator.getBiasXAsAcceleration(bx2);
27839 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
27840 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
27841 final var by1 = calibrator.getBiasYAsAcceleration();
27842 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
27843 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
27844 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27845 calibrator.getBiasYAsAcceleration(by2);
27846 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
27847 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
27848 final var bz1 = calibrator.getBiasZAsAcceleration();
27849 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
27850 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
27851 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27852 calibrator.getBiasZAsAcceleration(bz2);
27853 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
27854 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
27855 final var biasTriad1 = calibrator.getBiasAsTriad();
27856 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
27857 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
27858 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
27859 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
27860 final var biasTriad2 = new AccelerationTriad();
27861 calibrator.getBiasAsTriad(biasTriad2);
27862 assertEquals(biasTriad1, biasTriad2);
27863 assertEquals(sx, calibrator.getInitialSx(), 0.0);
27864 assertEquals(sy, calibrator.getInitialSy(), 0.0);
27865 assertEquals(sz, calibrator.getInitialSz(), 0.0);
27866 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
27867 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
27868 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
27869 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
27870 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
27871 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
27872 final var bias1 = calibrator.getBias();
27873 assertArrayEquals(bias1, bias, 0.0);
27874 final var bias2 = new double[3];
27875 calibrator.getBias(bias2);
27876 assertArrayEquals(bias1, bias2, 0.0);
27877 final var b1 = calibrator.getBiasAsMatrix();
27878 assertEquals(b1, ba);
27879 final var b2 = new Matrix(3, 1);
27880 calibrator.getBiasAsMatrix(b2);
27881 assertEquals(b1, b2);
27882 final var ma1 = new Matrix(3, 3);
27883 ma1.setSubmatrix(0, 0, 2, 2,
27884 new double[]{sx, myx, mzx,
27885 mxy, sy, mzy,
27886 mxz, myz, sz});
27887 assertEquals(ma1, calibrator.getInitialMa());
27888 final var ma2 = new Matrix(3, 3);
27889 calibrator.getInitialMa(ma2);
27890 assertEquals(ma1, ma2);
27891 assertSame(measurements, calibrator.getMeasurements());
27892 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
27893 calibrator.getMeasurementType());
27894 assertFalse(calibrator.isOrderedMeasurementsRequired());
27895 assertFalse(calibrator.isQualityScoresRequired());
27896 assertTrue(calibrator.isCommonAxisUsed());
27897 assertNull(calibrator.getListener());
27898 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
27899 assertFalse(calibrator.isReady());
27900 assertFalse(calibrator.isRunning());
27901 assertNull(calibrator.getEstimatedMa());
27902 assertNull(calibrator.getEstimatedSx());
27903 assertNull(calibrator.getEstimatedSy());
27904 assertNull(calibrator.getEstimatedSz());
27905 assertNull(calibrator.getEstimatedMxy());
27906 assertNull(calibrator.getEstimatedMxz());
27907 assertNull(calibrator.getEstimatedMyx());
27908 assertNull(calibrator.getEstimatedMyz());
27909 assertNull(calibrator.getEstimatedMzx());
27910 assertNull(calibrator.getEstimatedMzy());
27911 assertNull(calibrator.getEstimatedCovariance());
27912 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
27913 assertNotNull(calibrator.getGroundTruthGravityNorm());
27914 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27915 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27916 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
27917 ABSOLUTE_ERROR));
27918 final var g = new Acceleration(0.0, AccelerationUnit.G);
27919 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27920 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27921 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
27922
27923
27924 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27925
27926 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27927 invalidGravityNorm, measurements, true, ba, ma));
27928 final var m1 = new Matrix(1, 1);
27929 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27930 gravityNorm, measurements, true, m1, ma));
27931 final var m2 = new Matrix(1, 3);
27932 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27933 gravityNorm, measurements, true, m2, ma));
27934 final var m3 = new Matrix(1, 3);
27935 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27936 gravityNorm, measurements, true, ba, m3));
27937 final var m4 = new Matrix(3, 1);
27938 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27939 gravityNorm, measurements, true, ba, m4));
27940 }
27941
27942 @Test
27943 void testConstructor234() throws WrongSizeException {
27944 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
27945
27946 final var ba = generateBa();
27947 final var bias = ba.getBuffer();
27948 final var biasX = ba.getElementAtIndex(0);
27949 final var biasY = ba.getElementAtIndex(1);
27950 final var biasZ = ba.getElementAtIndex(2);
27951
27952 final var ma = generateMaCommonAxis();
27953 final var sx = ma.getElementAt(0, 0);
27954 final var sy = ma.getElementAt(1, 1);
27955 final var sz = ma.getElementAt(2, 2);
27956 final var mxy = ma.getElementAt(0, 1);
27957 final var mxz = ma.getElementAt(0, 2);
27958 final var myx = ma.getElementAt(1, 0);
27959 final var myz = ma.getElementAt(1, 2);
27960 final var mzx = ma.getElementAt(2, 0);
27961 final var mzy = ma.getElementAt(2, 1);
27962
27963 final var randomizer = new UniformRandomizer();
27964 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27965 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27966 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27967 final var nedPosition = new NEDPosition(latitude, longitude, height);
27968 final var nedVelocity = new NEDVelocity();
27969 final var ecefPosition = new ECEFPosition();
27970 final var ecefVelocity = new ECEFVelocity();
27971 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
27972 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27973 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27974 final var gravityNorm = gravity.getNormAsAcceleration();
27975
27976 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
27977 true, ba, ma, this);
27978
27979
27980 assertEquals(biasX, calibrator.getBiasX(), 0.0);
27981 assertEquals(biasY, calibrator.getBiasY(), 0.0);
27982 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
27983 final var bx1 = calibrator.getBiasXAsAcceleration();
27984 assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
27985 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
27986 final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27987 calibrator.getBiasXAsAcceleration(bx2);
27988 assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
27989 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
27990 final var by1 = calibrator.getBiasYAsAcceleration();
27991 assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
27992 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
27993 final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27994 calibrator.getBiasYAsAcceleration(by2);
27995 assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
27996 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
27997 final var bz1 = calibrator.getBiasZAsAcceleration();
27998 assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
27999 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
28000 final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
28001 calibrator.getBiasZAsAcceleration(bz2);
28002 assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
28003 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
28004 final var biasTriad1 = calibrator.getBiasAsTriad();
28005 assertEquals(biasX, biasTriad1.getValueX(), 0.0);
28006 assertEquals(biasY, biasTriad1.getValueY(), 0.0);
28007 assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
28008 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
28009 final var biasTriad2 = new AccelerationTriad();
28010 calibrator.getBiasAsTriad(biasTriad2);
28011 assertEquals(biasTriad1, biasTriad2);
28012 assertEquals(sx, calibrator.getInitialSx(), 0.0);
28013 assertEquals(sy, calibrator.getInitialSy(), 0.0);
28014 assertEquals(sz, calibrator.getInitialSz(), 0.0);
28015 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
28016 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
28017 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
28018 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
28019 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
28020 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
28021 final var bias1 = calibrator.getBias();
28022 assertArrayEquals(bias1, bias, 0.0);
28023 final var bias2 = new double[3];
28024 calibrator.getBias(bias2);
28025 assertArrayEquals(bias1, bias2, 0.0);
28026 final var b1 = calibrator.getBiasAsMatrix();
28027 assertEquals(b1, ba);
28028 final var b2 = new Matrix(3, 1);
28029 calibrator.getBiasAsMatrix(b2);
28030 assertEquals(b1, b2);
28031 final var ma1 = new Matrix(3, 3);
28032 ma1.setSubmatrix(0, 0, 2, 2,
28033 new double[]{sx, myx, mzx,
28034 mxy, sy, mzy,
28035 mxz, myz, sz});
28036 assertEquals(ma1, calibrator.getInitialMa());
28037 final var ma2 = new Matrix(3, 3);
28038 calibrator.getInitialMa(ma2);
28039 assertEquals(ma1, ma2);
28040 assertSame(measurements, calibrator.getMeasurements());
28041 assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
28042 calibrator.getMeasurementType());
28043 assertFalse(calibrator.isOrderedMeasurementsRequired());
28044 assertFalse(calibrator.isQualityScoresRequired());
28045 assertTrue(calibrator.isCommonAxisUsed());
28046 assertSame(this, calibrator.getListener());
28047 assertEquals(7, calibrator.getMinimumRequiredMeasurements());
28048 assertFalse(calibrator.isReady());
28049 assertFalse(calibrator.isRunning());
28050 assertNull(calibrator.getEstimatedMa());
28051 assertNull(calibrator.getEstimatedSx());
28052 assertNull(calibrator.getEstimatedSy());
28053 assertNull(calibrator.getEstimatedSz());
28054 assertNull(calibrator.getEstimatedMxy());
28055 assertNull(calibrator.getEstimatedMxz());
28056 assertNull(calibrator.getEstimatedMyx());
28057 assertNull(calibrator.getEstimatedMyz());
28058 assertNull(calibrator.getEstimatedMzx());
28059 assertNull(calibrator.getEstimatedMzy());
28060 assertNull(calibrator.getEstimatedCovariance());
28061 assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
28062 assertNotNull(calibrator.getGroundTruthGravityNorm());
28063 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28064 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28065 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
28066 ABSOLUTE_ERROR));
28067 final var g = new Acceleration(0.0, AccelerationUnit.G);
28068 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28069 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28070 assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
28071
28072
28073 final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28074
28075 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
28076 invalidGravityNorm, measurements, true, ba, ma, this));
28077 final var m1 = new Matrix(1, 1);
28078 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
28079 gravityNorm, measurements, true, m1, ma, this));
28080 final var m2 = new Matrix(1, 3);
28081 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
28082 gravityNorm, measurements, true, m2, ma, this));
28083 final var m3 = new Matrix(1, 3);
28084 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
28085 gravityNorm, measurements, true, ba, m3, this));
28086 final var m4 = new Matrix(3, 1);
28087 assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
28088 gravityNorm, measurements, true, ba, m4, this));
28089 }
28090
28091 @Test
28092 void testGetSetBiasX() throws LockedException {
28093 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28094
28095 assertEquals(0.0, calibrator.getBiasX(), 0.0);
28096
28097
28098 final var ba = generateBa();
28099 final var biasX = ba.getElementAtIndex(0);
28100
28101 calibrator.setBiasX(biasX);
28102
28103
28104 assertEquals(biasX, calibrator.getBiasX(), 0.0);
28105 }
28106
28107 @Test
28108 void testGetSetBiasY() throws LockedException {
28109 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28110
28111 assertEquals(0.0, calibrator.getBiasY(), 0.0);
28112
28113
28114 final var ba = generateBa();
28115 final var biasY = ba.getElementAtIndex(1);
28116
28117 calibrator.setBiasY(biasY);
28118
28119
28120 assertEquals(biasY, calibrator.getBiasY(), 0.0);
28121 }
28122
28123 @Test
28124 void testGetSetBiasZ() throws LockedException {
28125 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28126
28127 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
28128
28129
28130 final var ba = generateBa();
28131 final var biasZ = ba.getElementAtIndex(2);
28132
28133 calibrator.setBiasZ(biasZ);
28134
28135
28136 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
28137 }
28138
28139 @Test
28140 void testGetSetBiasAsTriad() throws LockedException {
28141 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28142
28143
28144 final var triad1 = calibrator.getBiasAsTriad();
28145 assertEquals(0.0, triad1.getValueX(), 0.0);
28146 assertEquals(0.0, triad1.getValueY(), 0.0);
28147 assertEquals(0.0, triad1.getValueZ(), 0.0);
28148 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, triad1.getUnit());
28149
28150
28151 final var ba = generateBa();
28152 final var biasX = ba.getElementAtIndex(0);
28153 final var biasY = ba.getElementAtIndex(1);
28154 final var biasZ = ba.getElementAtIndex(2);
28155
28156 final var triad2 = new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
28157 calibrator.setBias(triad2);
28158
28159
28160 final var triad3 = calibrator.getBiasAsTriad();
28161 final var triad4 = new AccelerationTriad();
28162 calibrator.getBiasAsTriad(triad4);
28163
28164 assertEquals(triad2, triad3);
28165 assertEquals(triad2, triad4);
28166 }
28167
28168 @Test
28169 void testGetSetBiasXAsAcceleration() throws LockedException {
28170 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28171
28172
28173 final var bx1 = calibrator.getBiasXAsAcceleration();
28174
28175 assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
28176 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
28177
28178
28179 final var ba = generateBa();
28180 final var biasX = ba.getElementAtIndex(0);
28181
28182 final var bx2 = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28183 calibrator.setBiasX(bx2);
28184
28185
28186 final var bx3 = calibrator.getBiasXAsAcceleration();
28187 final var bx4 = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28188 calibrator.getBiasXAsAcceleration(bx4);
28189
28190 assertEquals(bx3, bx4);
28191 }
28192
28193 @Test
28194 void testGetSetBiasYAsAcceleration() throws LockedException {
28195 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28196
28197
28198 final var by1 = calibrator.getBiasYAsAcceleration();
28199
28200 assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
28201 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
28202
28203
28204 final var ba = generateBa();
28205 final var biasY = ba.getElementAtIndex(1);
28206
28207 final var by2 = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28208 calibrator.setBiasY(by2);
28209
28210
28211 final var by3 = calibrator.getBiasYAsAcceleration();
28212 final var by4 = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28213 calibrator.getBiasYAsAcceleration(by4);
28214
28215 assertEquals(by3, by4);
28216 }
28217
28218 @Test
28219 void testGetSetBiasZAsAcceleration() throws LockedException {
28220 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28221
28222
28223 final var bz1 = calibrator.getBiasZAsAcceleration();
28224
28225 assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
28226 assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
28227
28228
28229 final var ba = generateBa();
28230 final var biasZ = ba.getElementAtIndex(2);
28231
28232 final var bz2 = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28233 calibrator.setBiasZ(bz2);
28234
28235
28236 final var bz3 = calibrator.getBiasZAsAcceleration();
28237 final var bz4 = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28238 calibrator.getBiasZAsAcceleration(bz4);
28239
28240 assertEquals(bz3, bz4);
28241 }
28242
28243 @Test
28244 void testSetBiasCoordinates1() throws LockedException {
28245 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28246
28247
28248 assertEquals(0.0, calibrator.getBiasX(), 0.0);
28249 assertEquals(0.0, calibrator.getBiasY(), 0.0);
28250 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
28251
28252
28253 final var ba = generateBa();
28254 final var biasX = ba.getElementAtIndex(0);
28255 final var biasY = ba.getElementAtIndex(1);
28256 final var biasZ = ba.getElementAtIndex(2);
28257
28258 calibrator.setBiasCoordinates(biasX, biasY, biasZ);
28259
28260
28261 assertEquals(biasX, calibrator.getBiasX(), 0.0);
28262 assertEquals(biasY, calibrator.getBiasY(), 0.0);
28263 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
28264 }
28265
28266 @Test
28267 void testSetBias2() throws LockedException {
28268 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28269
28270
28271 assertEquals(0.0, calibrator.getBiasX(), 0.0);
28272 assertEquals(0.0, calibrator.getBiasY(), 0.0);
28273 assertEquals(0.0, calibrator.getBiasZ(), 0.0);
28274
28275
28276 final var ba = generateBa();
28277 final var biasX = ba.getElementAtIndex(0);
28278 final var biasY = ba.getElementAtIndex(1);
28279 final var biasZ = ba.getElementAtIndex(2);
28280
28281 final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28282 final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28283 final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28284
28285 calibrator.setBiasCoordinates(bx, by, bz);
28286
28287
28288 assertEquals(biasX, calibrator.getBiasX(), 0.0);
28289 assertEquals(biasY, calibrator.getBiasY(), 0.0);
28290 assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
28291 }
28292
28293 @Test
28294 void testGetSetInitialSx() throws WrongSizeException, LockedException {
28295 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28296
28297 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
28298
28299
28300 final var ma = generateMaGeneral();
28301 final var sx = ma.getElementAt(0, 0);
28302
28303 calibrator.setInitialSx(sx);
28304
28305
28306 assertEquals(sx, calibrator.getInitialSx(), 0.0);
28307 }
28308
28309 @Test
28310 void testGetSetInitialSy() throws WrongSizeException, LockedException {
28311 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28312
28313 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
28314
28315
28316 final var ma = generateMaGeneral();
28317 final var sy = ma.getElementAt(1, 1);
28318
28319 calibrator.setInitialSy(sy);
28320
28321
28322 assertEquals(sy, calibrator.getInitialSy(), 0.0);
28323 }
28324
28325 @Test
28326 void testGetSetInitialSz() throws WrongSizeException, LockedException {
28327 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28328
28329 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
28330
28331
28332 final var ma = generateMaGeneral();
28333 final var sz = ma.getElementAt(2, 2);
28334
28335 calibrator.setInitialSz(sz);
28336
28337
28338 assertEquals(sz, calibrator.getInitialSz(), 0.0);
28339 }
28340
28341 @Test
28342 void testGetSetInitialMxy() throws WrongSizeException, LockedException {
28343 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28344
28345 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
28346
28347
28348 final var ma = generateMaGeneral();
28349 final var mxy = ma.getElementAt(0, 1);
28350
28351 calibrator.setInitialMxy(mxy);
28352
28353
28354 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
28355 }
28356
28357 @Test
28358 void testGetSetInitialMxz() throws WrongSizeException, LockedException {
28359 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28360
28361 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
28362
28363
28364 final var ma = generateMaGeneral();
28365 final var mxz = ma.getElementAt(0, 2);
28366
28367 calibrator.setInitialMxz(mxz);
28368
28369
28370 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28371 }
28372
28373 @Test
28374 void testGetSetInitialMyx() throws WrongSizeException, LockedException {
28375 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28376
28377 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
28378
28379
28380 final var ma = generateMaGeneral();
28381 final var myx = ma.getElementAt(1, 0);
28382
28383 calibrator.setInitialMyx(myx);
28384
28385
28386 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
28387 }
28388
28389 @Test
28390 void testGetSetInitialMyz() throws WrongSizeException, LockedException {
28391 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28392
28393 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
28394
28395
28396 final var ma = generateMaGeneral();
28397 final var myz = ma.getElementAt(1, 2);
28398
28399 calibrator.setInitialMyz(myz);
28400
28401
28402 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
28403 }
28404
28405 @Test
28406 void testGetSetInitialMzx() throws WrongSizeException, LockedException {
28407 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28408
28409 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
28410
28411
28412 final var ma = generateMaGeneral();
28413 final var mzx = ma.getElementAt(2, 0);
28414
28415 calibrator.setInitialMzx(mzx);
28416
28417
28418 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
28419 }
28420
28421 @Test
28422 void testGetSetInitialMzy() throws WrongSizeException, LockedException {
28423 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28424
28425 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
28426
28427
28428 final var ma = generateMaGeneral();
28429 final var mzy = ma.getElementAt(2, 1);
28430
28431 calibrator.setInitialMzy(mzy);
28432
28433
28434 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
28435 }
28436
28437 @Test
28438 void testSetInitialScalingFactors() throws WrongSizeException, LockedException {
28439
28440 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28441
28442
28443 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
28444 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
28445 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
28446
28447
28448 final var ma = generateMaGeneral();
28449 final var sx = ma.getElementAt(0, 0);
28450 final var sy = ma.getElementAt(1, 1);
28451 final var sz = ma.getElementAt(2, 2);
28452
28453 calibrator.setInitialScalingFactors(sx, sy, sz);
28454
28455
28456 assertEquals(sx, calibrator.getInitialSx(), 0.0);
28457 assertEquals(sy, calibrator.getInitialSy(), 0.0);
28458 assertEquals(sz, calibrator.getInitialSz(), 0.0);
28459 }
28460
28461 @Test
28462 void testSetInitialCrossCouplingErrors() throws WrongSizeException, LockedException {
28463
28464 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28465
28466
28467 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
28468 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
28469 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
28470 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
28471 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
28472 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
28473
28474
28475 final var ma = generateMaGeneral();
28476 final var mxy = ma.getElementAt(0, 1);
28477 final var mxz = ma.getElementAt(0, 2);
28478 final var myx = ma.getElementAt(1, 0);
28479 final var myz = ma.getElementAt(1, 2);
28480 final var mzx = ma.getElementAt(2, 0);
28481 final var mzy = ma.getElementAt(2, 1);
28482
28483 calibrator.setInitialCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
28484
28485
28486 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
28487 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
28488 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
28489 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
28490 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
28491 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
28492 }
28493
28494 @Test
28495 void testSetInitialScalingFactorsAndCrossCouplingErrors() throws WrongSizeException, LockedException {
28496
28497 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28498
28499
28500 assertEquals(0.0, calibrator.getInitialSx(), 0.0);
28501 assertEquals(0.0, calibrator.getInitialSy(), 0.0);
28502 assertEquals(0.0, calibrator.getInitialSz(), 0.0);
28503 assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
28504 assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
28505 assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
28506 assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
28507 assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
28508 assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
28509
28510
28511 final var ma = generateMaGeneral();
28512 final var sx = ma.getElementAt(0, 0);
28513 final var sy = ma.getElementAt(1, 1);
28514 final var sz = ma.getElementAt(2, 2);
28515 final var mxy = ma.getElementAt(0, 1);
28516 final var mxz = ma.getElementAt(0, 2);
28517 final var myx = ma.getElementAt(1, 0);
28518 final var myz = ma.getElementAt(1, 2);
28519 final var mzx = ma.getElementAt(2, 0);
28520 final var mzy = ma.getElementAt(2, 1);
28521
28522 calibrator.setInitialScalingFactorsAndCrossCouplingErrors(sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
28523
28524
28525 assertEquals(sx, calibrator.getInitialSx(), 0.0);
28526 assertEquals(sy, calibrator.getInitialSy(), 0.0);
28527 assertEquals(sz, calibrator.getInitialSz(), 0.0);
28528 assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
28529 assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
28530 assertEquals(myx, calibrator.getInitialMyx(), 0.0);
28531 assertEquals(myz, calibrator.getInitialMyz(), 0.0);
28532 assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
28533 assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
28534 }
28535
28536 @Test
28537 void testGetSetBias() throws LockedException {
28538 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28539
28540
28541 final var bias1 = calibrator.getBias();
28542 assertArrayEquals(new double[3], bias1, 0.0);
28543
28544
28545 final var ba = generateBa();
28546 final var bias2 = ba.getBuffer();
28547 calibrator.setBias(bias2);
28548
28549
28550 final var bias3 = calibrator.getBias();
28551 final var bias4 = new double[3];
28552 calibrator.getBias(bias4);
28553
28554 assertArrayEquals(bias2, bias3, 0.0);
28555 assertArrayEquals(bias2, bias4, 0.0);
28556
28557
28558 assertThrows(IllegalArgumentException.class, () -> calibrator.getBias(new double[1]));
28559 assertThrows(IllegalArgumentException.class, () -> calibrator.setBias(new double[1]));
28560 }
28561
28562 @Test
28563 void testGetSetBiasAsMatrix() throws WrongSizeException, LockedException {
28564
28565 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28566
28567
28568 final var bias1 = calibrator.getBiasAsMatrix();
28569 assertEquals(new Matrix(3, 1), bias1);
28570
28571
28572 final var bias2 = generateBa();
28573 calibrator.setBias(bias2);
28574
28575
28576 final var bias3 = calibrator.getBiasAsMatrix();
28577 final var bias4 = new Matrix(3, 1);
28578 calibrator.getBiasAsMatrix(bias4);
28579
28580 assertEquals(bias2, bias3);
28581 assertEquals(bias2, bias4);
28582
28583
28584 final var m1 = new Matrix(1, 1);
28585 assertThrows(IllegalArgumentException.class, () -> calibrator.getBiasAsMatrix(m1));
28586 final var m2 = new Matrix(3, 3);
28587 assertThrows(IllegalArgumentException.class, () -> calibrator.getBiasAsMatrix(m2));
28588 final var m3 = new Matrix(1, 1);
28589 assertThrows(IllegalArgumentException.class, () -> calibrator.setBias(m3));
28590 final var m4 = new Matrix(3, 3);
28591 assertThrows(IllegalArgumentException.class, () -> calibrator.setBias(m4));
28592 }
28593
28594 @Test
28595 void testGetSetInitialMa() throws WrongSizeException, LockedException {
28596 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28597
28598
28599 final var ma1 = calibrator.getInitialMa();
28600 assertEquals(new Matrix(3, 3), ma1);
28601
28602
28603 final var ma2 = generateMaGeneral();
28604 calibrator.setInitialMa(ma2);
28605
28606
28607 final var ma3 = calibrator.getInitialMa();
28608 final var ma4 = new Matrix(3, 3);
28609 calibrator.getInitialMa(ma4);
28610
28611 assertEquals(ma2, ma3);
28612 assertEquals(ma2, ma4);
28613
28614
28615 final var m1 = new Matrix(1, 3);
28616 assertThrows(IllegalArgumentException.class, () -> calibrator.getInitialMa(m1));
28617 final var m2 = new Matrix(3, 1);
28618 assertThrows(IllegalArgumentException.class, () -> calibrator.getInitialMa(m2));
28619 final var m3 = new Matrix(1, 3);
28620 assertThrows(IllegalArgumentException.class, () -> calibrator.setInitialMa(m3));
28621 final var m4 = new Matrix(3, 1);
28622 assertThrows(IllegalArgumentException.class, () -> calibrator.setInitialMa(m4));
28623 }
28624
28625 @Test
28626 void testGetSetMeasurements() throws LockedException {
28627 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28628
28629
28630 assertNull(calibrator.getMeasurements());
28631
28632
28633 final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
28634 calibrator.setMeasurements(measurements);
28635
28636
28637 assertSame(measurements, calibrator.getMeasurements());
28638 }
28639
28640 @Test
28641 void testIsSetCommonAxisUsed() throws LockedException {
28642 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28643
28644
28645 assertFalse(calibrator.isCommonAxisUsed());
28646
28647
28648 calibrator.setCommonAxisUsed(true);
28649
28650
28651 assertTrue(calibrator.isCommonAxisUsed());
28652 }
28653
28654 @Test
28655 void testGetSetListener() throws LockedException {
28656 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28657
28658
28659 assertNull(calibrator.getListener());
28660
28661
28662 calibrator.setListener(this);
28663
28664
28665 assertSame(this, calibrator.getListener());
28666 }
28667
28668 @Test
28669 void testGetSetGroundTruthGravityNorm1() throws LockedException {
28670 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28671
28672
28673 assertNull(calibrator.getGroundTruthGravityNorm());
28674
28675
28676 final var randomizer = new UniformRandomizer();
28677 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28678 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28679 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28680 final var nedPosition = new NEDPosition(latitude, longitude, height);
28681 final var nedVelocity = new NEDVelocity();
28682 final var ecefPosition = new ECEFPosition();
28683 final var ecefVelocity = new ECEFVelocity();
28684 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
28685 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28686 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28687 final var gravityNorm = gravity.getNorm();
28688
28689 calibrator.setGroundTruthGravityNorm(gravityNorm);
28690
28691
28692 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), 0.0);
28693 assertEquals(gravity.getNormAsAcceleration(), calibrator.getGroundTruthGravityNormAsAcceleration());
28694 final var g = new Acceleration(0.0, AccelerationUnit.G);
28695 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28696 assertEquals(g, gravity.getNormAsAcceleration());
28697 }
28698
28699 @Test
28700 void testGetSetGroundTruthGravityNorm2() throws LockedException {
28701 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28702
28703
28704 assertNull(calibrator.getGroundTruthGravityNorm());
28705
28706
28707 final var randomizer = new UniformRandomizer();
28708 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28709 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28710 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28711 final var nedPosition = new NEDPosition(latitude, longitude, height);
28712 final var nedVelocity = new NEDVelocity();
28713 final var ecefPosition = new ECEFPosition();
28714 final var ecefVelocity = new ECEFVelocity();
28715 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
28716 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28717 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28718 final var gravityNorm = gravity.getNormAsAcceleration();
28719
28720 calibrator.setGroundTruthGravityNorm(gravityNorm);
28721
28722
28723 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), 0.0);
28724 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNormAsAcceleration());
28725 final var g = new Acceleration(0.0, AccelerationUnit.G);
28726 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28727 assertEquals(gravityNorm, g);
28728 }
28729
28730 @Test
28731 void testIsReady() throws LockedException {
28732 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28733
28734
28735 assertFalse(calibrator.isReady());
28736
28737
28738 final var measurements1 = Collections.<StandardDeviationBodyKinematics>emptyList();
28739 calibrator.setMeasurements(measurements1);
28740
28741
28742 assertFalse(calibrator.isReady());
28743
28744
28745 calibrator.setCommonAxisUsed(false);
28746
28747 final var measurements2 = new ArrayList<StandardDeviationBodyKinematics>();
28748 for (int i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
28749 measurements2.add(new StandardDeviationBodyKinematics());
28750 }
28751 calibrator.setMeasurements(measurements2);
28752
28753
28754 assertFalse(calibrator.isReady());
28755
28756
28757 final var randomizer = new UniformRandomizer();
28758 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28759 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28760 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28761 final var nedPosition = new NEDPosition(latitude, longitude, height);
28762 final var nedVelocity = new NEDVelocity();
28763 final var ecefPosition = new ECEFPosition();
28764 final var ecefVelocity = new ECEFVelocity();
28765 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
28766 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28767 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28768
28769 calibrator.setGroundTruthGravityNorm(gravity.getNorm());
28770
28771 assertTrue(calibrator.isReady());
28772
28773
28774 measurements2.clear();
28775 for (var i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS; i++) {
28776 measurements2.add(new StandardDeviationBodyKinematics());
28777 }
28778 calibrator.setMeasurements(measurements2);
28779
28780
28781 assertFalse(calibrator.isReady());
28782
28783 calibrator.setCommonAxisUsed(true);
28784
28785 assertTrue(calibrator.isReady());
28786 }
28787
28788 @Test
28789 void testCalibrateForGeneralCaseWithMinimumMeasuresAndNoNoise() throws WrongSizeException,
28790 InvalidSourceAndDestinationFrameTypeException, LockedException, NotReadyException {
28791
28792 var numValid = 0;
28793 for (var t = 0; t < TIMES; t++) {
28794 final var ba = generateBa();
28795 final var bg = generateBg();
28796 final var ma = generateMaGeneral();
28797 final var mg = generateMg();
28798 final var gg = generateGg();
28799
28800
28801 final var accelNoiseRootPSD = 0.0;
28802 final var gyroNoiseRootPSD = 0.0;
28803 final var accelQuantLevel = 0.0;
28804 final var gyroQuantLevel = 0.0;
28805
28806 final var errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD, gyroNoiseRootPSD, accelQuantLevel,
28807 gyroQuantLevel);
28808
28809 final var randomizer = new UniformRandomizer();
28810 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28811 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28812 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28813 final var nedPosition = new NEDPosition(latitude, longitude, height);
28814 final var nedVelocity = new NEDVelocity();
28815 final var ecefPosition = new ECEFPosition();
28816 final var ecefVelocity = new ECEFVelocity();
28817 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
28818 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28819 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28820
28821 final var sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
28822 final var specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
28823 final var angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
28824
28825 final var measurements = new ArrayList<StandardDeviationBodyKinematics>();
28826 for (var i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
28827 final var roll = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
28828 final var pitch = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
28829 final var yaw = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
28830 final var nedC = new CoordinateTransformation(roll, pitch, yaw,
28831 FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
28832
28833 final var nedFrame = new NEDFrame(nedPosition, nedC);
28834 final var ecefFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
28835
28836
28837
28838 final var trueKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(
28839 TIME_INTERVAL_SECONDS, ecefFrame, ecefFrame);
28840
28841
28842
28843 final var random = new Random();
28844 final var measuredKinematics = BodyKinematicsGenerator.generate(TIME_INTERVAL_SECONDS, trueKinematics,
28845 errors, random);
28846
28847 final var measurement = new StandardDeviationBodyKinematics(measuredKinematics,
28848 specificForceStandardDeviation, angularRateStandardDeviation);
28849 measurements.add(measurement);
28850 }
28851
28852
28853
28854 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravity.getNorm(), measurements,
28855 false, ba, ma, this);
28856
28857
28858 reset();
28859 assertTrue(calibrator.isReady());
28860 assertFalse(calibrator.isRunning());
28861 assertEquals(0, calibrateStart);
28862 assertEquals(0, calibrateEnd);
28863
28864 try {
28865 calibrator.calibrate();
28866 } catch (final CalibrationException e) {
28867 continue;
28868 }
28869
28870
28871 assertTrue(calibrator.isReady());
28872 assertFalse(calibrator.isRunning());
28873 assertEquals(1, calibrateStart);
28874 assertEquals(1, calibrateEnd);
28875
28876 final var estimatedMa = calibrator.getEstimatedMa();
28877
28878 if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
28879 continue;
28880 }
28881
28882 assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
28883
28884 assertEstimatedResult(estimatedMa, calibrator);
28885
28886 assertNotNull(calibrator.getEstimatedCovariance());
28887 checkGeneralCovariance(calibrator.getEstimatedCovariance());
28888 assertTrue(calibrator.getEstimatedMse() >= 0.0);
28889
28890 numValid++;
28891 break;
28892 }
28893
28894 assertTrue(numValid > 0);
28895 }
28896
28897 @Test
28898 void testCalibrateForGeneralCaseWithLargeNumberOfMeasurementsAndNoise() throws WrongSizeException,
28899 InvalidSourceAndDestinationFrameTypeException, LockedException, NotReadyException {
28900
28901 var numValid = 0;
28902 for (var t = 0; t < TIMES; t++) {
28903 final var ba = generateBa();
28904 final var bg = generateBg();
28905 final var ma = generateMaGeneral();
28906 final var mg = generateMg();
28907 final var gg = generateGg();
28908 final var accelNoiseRootPSD = getAccelNoiseRootPSD();
28909 final var gyroNoiseRootPSD = getGyroNoiseRootPSD();
28910 final var accelQuantLevel = 0.0;
28911 final var gyroQuantLevel = 0.0;
28912
28913 final var errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD, gyroNoiseRootPSD, accelQuantLevel,
28914 gyroQuantLevel);
28915
28916 final var randomizer = new UniformRandomizer();
28917 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28918 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28919 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28920 final var nedPosition = new NEDPosition(latitude, longitude, height);
28921 final var nedVelocity = new NEDVelocity();
28922 final var ecefPosition = new ECEFPosition();
28923 final var ecefVelocity = new ECEFVelocity();
28924 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
28925 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28926 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28927
28928 final var sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
28929 final var specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
28930 final var angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
28931
28932 final var measurements = new ArrayList<StandardDeviationBodyKinematics>();
28933 for (var i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
28934 final var roll = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
28935 final var pitch = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
28936 final var yaw = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
28937 final var nedC = new CoordinateTransformation(roll, pitch, yaw, FrameType.BODY_FRAME,
28938 FrameType.LOCAL_NAVIGATION_FRAME);
28939
28940 final var nedFrame = new NEDFrame(nedPosition, nedC);
28941 final var ecefFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
28942
28943
28944
28945 final var trueKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS,
28946 ecefFrame, ecefFrame);
28947
28948
28949
28950 final var random = new Random();
28951 final var measuredKinematics = BodyKinematicsGenerator.generate(TIME_INTERVAL_SECONDS, trueKinematics,
28952 errors, random);
28953
28954 final var measurement = new StandardDeviationBodyKinematics(measuredKinematics,
28955 specificForceStandardDeviation, angularRateStandardDeviation);
28956 measurements.add(measurement);
28957 }
28958
28959
28960
28961 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravity.getNorm(), measurements,
28962 false, this);
28963
28964
28965 reset();
28966 assertTrue(calibrator.isReady());
28967 assertFalse(calibrator.isRunning());
28968 assertEquals(0, calibrateStart);
28969 assertEquals(0, calibrateEnd);
28970
28971 try {
28972 calibrator.calibrate();
28973 } catch (final CalibrationException e) {
28974 continue;
28975 }
28976
28977
28978 assertTrue(calibrator.isReady());
28979 assertFalse(calibrator.isRunning());
28980 assertEquals(1, calibrateStart);
28981 assertEquals(1, calibrateEnd);
28982
28983 final var estimatedMa = calibrator.getEstimatedMa();
28984
28985 if (!ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR)) {
28986 continue;
28987 }
28988
28989 assertTrue(ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR));
28990
28991 assertEstimatedResult(estimatedMa, calibrator);
28992
28993 assertNotNull(calibrator.getEstimatedCovariance());
28994 checkGeneralCovariance(calibrator.getEstimatedCovariance());
28995 assertTrue(calibrator.getEstimatedMse() > 0.0);
28996
28997 numValid++;
28998 break;
28999 }
29000
29001 assertTrue(numValid > 0);
29002 }
29003
29004 @Test
29005 void testCalibrateForCommonAxisCaseWithMinimumMeasuresAndNoNoise() throws WrongSizeException,
29006 InvalidSourceAndDestinationFrameTypeException, LockedException, NotReadyException {
29007
29008 var numValid = 0;
29009 for (var t = 0; t < TIMES; t++) {
29010 final var ba = generateBa();
29011 final var bg = generateBg();
29012 final var ma = generateMaCommonAxis();
29013 final var mg = generateMg();
29014 final var gg = generateGg();
29015
29016
29017 final var accelNoiseRootPSD = 0.0;
29018 final var gyroNoiseRootPSD = 0.0;
29019 final var accelQuantLevel = 0.0;
29020 final var gyroQuantLevel = 0.0;
29021
29022 final var errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD, gyroNoiseRootPSD, accelQuantLevel,
29023 gyroQuantLevel);
29024
29025 final var randomizer = new UniformRandomizer();
29026 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29027 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29028 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29029 final var nedPosition = new NEDPosition(latitude, longitude, height);
29030 final var nedVelocity = new NEDVelocity();
29031 final var ecefPosition = new ECEFPosition();
29032 final var ecefVelocity = new ECEFVelocity();
29033 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
29034 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29035 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29036
29037 final var sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29038 final var specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29039 final var angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29040
29041 final var measurements = new ArrayList<StandardDeviationBodyKinematics>();
29042 for (var i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS; i++) {
29043 final var roll = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29044 final var pitch = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29045 final var yaw = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29046 final var nedC = new CoordinateTransformation(roll, pitch, yaw, FrameType.BODY_FRAME,
29047 FrameType.LOCAL_NAVIGATION_FRAME);
29048
29049 final var nedFrame = new NEDFrame(nedPosition, nedC);
29050 final var ecefFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
29051
29052
29053
29054 final var trueKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS,
29055 ecefFrame, ecefFrame);
29056
29057
29058
29059 final var random = new Random();
29060 final var measuredKinematics = BodyKinematicsGenerator.generate(TIME_INTERVAL_SECONDS, trueKinematics,
29061 errors, random);
29062
29063 final var measurement = new StandardDeviationBodyKinematics(measuredKinematics,
29064 specificForceStandardDeviation, angularRateStandardDeviation);
29065 measurements.add(measurement);
29066 }
29067
29068
29069
29070 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravity.getNorm(), measurements,
29071 true, ba, ma, this);
29072
29073
29074 reset();
29075 assertTrue(calibrator.isReady());
29076 assertFalse(calibrator.isRunning());
29077 assertEquals(0, calibrateStart);
29078 assertEquals(0, calibrateEnd);
29079
29080 try {
29081 calibrator.calibrate();
29082 } catch (final CalibrationException e) {
29083 continue;
29084 }
29085
29086
29087 assertTrue(calibrator.isReady());
29088 assertFalse(calibrator.isRunning());
29089 assertEquals(1, calibrateStart);
29090 assertEquals(1, calibrateEnd);
29091
29092 final var estimatedMa = calibrator.getEstimatedMa();
29093
29094 if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
29095 continue;
29096 }
29097
29098 assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
29099
29100 assertEstimatedResult(estimatedMa, calibrator);
29101
29102 assertNotNull(calibrator.getEstimatedCovariance());
29103 checkCommonAxisCovariance(calibrator.getEstimatedCovariance());
29104 assertTrue(calibrator.getEstimatedMse() >= 0.0);
29105
29106 numValid++;
29107
29108 break;
29109 }
29110
29111 assertTrue(numValid > 0);
29112 }
29113
29114 @Test
29115 void testCalibrateForCommonAxisCaseWithLargeNumberOfMeasurementsAndNoise() throws WrongSizeException,
29116 InvalidSourceAndDestinationFrameTypeException, LockedException, NotReadyException {
29117
29118 var numValid = 0;
29119 for (var t = 0; t < TIMES; t++) {
29120 final var ba = generateBa();
29121 final var bg = generateBg();
29122 final var ma = generateMaCommonAxis();
29123 final var mg = generateMg();
29124 final var gg = generateGg();
29125 final var accelNoiseRootPSD = getAccelNoiseRootPSD();
29126 final var gyroNoiseRootPSD = getGyroNoiseRootPSD();
29127 final var accelQuantLevel = 0.0;
29128 final var gyroQuantLevel = 0.0;
29129
29130 final var errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD, gyroNoiseRootPSD, accelQuantLevel,
29131 gyroQuantLevel);
29132
29133 final var randomizer = new UniformRandomizer();
29134 final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29135 final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29136 final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29137 final var nedPosition = new NEDPosition(latitude, longitude, height);
29138 final var nedVelocity = new NEDVelocity();
29139 final var ecefPosition = new ECEFPosition();
29140 final var ecefVelocity = new ECEFVelocity();
29141 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
29142 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29143 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29144
29145 final var sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29146 final var specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29147 final var angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29148
29149 final var measurements = new ArrayList<StandardDeviationBodyKinematics>();
29150 for (var i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
29151 final var roll = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29152 final var pitch = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29153 final var yaw = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29154 final var nedC = new CoordinateTransformation(roll, pitch, yaw, FrameType.BODY_FRAME,
29155 FrameType.LOCAL_NAVIGATION_FRAME);
29156
29157 final var nedFrame = new NEDFrame(nedPosition, nedC);
29158 final var ecefFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
29159
29160
29161
29162 final var trueKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS,
29163 ecefFrame, ecefFrame);
29164
29165
29166
29167 final var random = new Random();
29168 final var measuredKinematics = BodyKinematicsGenerator.generate(TIME_INTERVAL_SECONDS, trueKinematics,
29169 errors, random);
29170
29171 final var measurement = new StandardDeviationBodyKinematics(measuredKinematics,
29172 specificForceStandardDeviation, angularRateStandardDeviation);
29173 measurements.add(measurement);
29174 }
29175
29176
29177
29178 final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravity.getNorm(), measurements,
29179 true, this);
29180
29181
29182 reset();
29183 assertTrue(calibrator.isReady());
29184 assertFalse(calibrator.isRunning());
29185 assertEquals(0, calibrateStart);
29186 assertEquals(0, calibrateEnd);
29187
29188 try {
29189 calibrator.calibrate();
29190 } catch (final CalibrationException e) {
29191 continue;
29192 }
29193
29194
29195 assertTrue(calibrator.isReady());
29196 assertFalse(calibrator.isRunning());
29197 assertEquals(1, calibrateStart);
29198 assertEquals(1, calibrateEnd);
29199
29200 final var estimatedMa = calibrator.getEstimatedMa();
29201
29202 if (!ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR)) {
29203 continue;
29204 }
29205
29206 assertTrue(ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR));
29207
29208 assertEstimatedResult(estimatedMa, calibrator);
29209
29210 assertNotNull(calibrator.getEstimatedCovariance());
29211 checkCommonAxisCovariance(calibrator.getEstimatedCovariance());
29212 assertTrue(calibrator.getEstimatedMse() > 0.0);
29213
29214 numValid++;
29215 break;
29216 }
29217
29218 assertTrue(numValid > 0);
29219 }
29220
29221 @Override
29222 public void onCalibrateStart(final KnownBiasAndGravityNormAccelerometerCalibrator calibrator) {
29223 checkLocked(calibrator);
29224 calibrateStart++;
29225 }
29226
29227 @Override
29228 public void onCalibrateEnd(final KnownBiasAndGravityNormAccelerometerCalibrator calibrator) {
29229 checkLocked(calibrator);
29230 calibrateEnd++;
29231 }
29232
29233 private void reset() {
29234 calibrateStart = 0;
29235 calibrateEnd = 0;
29236 }
29237
29238 private void checkLocked(final KnownBiasAndGravityNormAccelerometerCalibrator calibrator) {
29239 assertTrue(calibrator.isRunning());
29240 assertThrows(LockedException.class, () -> calibrator.setBiasX(0.0));
29241 assertThrows(LockedException.class, () -> calibrator.setBiasY(0.0));
29242 assertThrows(LockedException.class, () -> calibrator.setBiasZ(0.0));
29243 assertThrows(LockedException.class, () -> calibrator.setBiasX(null));
29244 assertThrows(LockedException.class, () -> calibrator.setBiasY(null));
29245 assertThrows(LockedException.class, () -> calibrator.setBiasZ(null));
29246 assertThrows(LockedException.class, () -> calibrator.setBiasCoordinates(0.0, 0.0, 0.0));
29247 assertThrows(LockedException.class, () -> calibrator.setBiasCoordinates(null, null, null));
29248 assertThrows(LockedException.class, () -> calibrator.setBias((AccelerationTriad) null));
29249 assertThrows(LockedException.class, () -> calibrator.setInitialSx(0.0));
29250 assertThrows(LockedException.class, () -> calibrator.setInitialSy(0.0));
29251 assertThrows(LockedException.class, () -> calibrator.setInitialSz(0.0));
29252 assertThrows(LockedException.class, () -> calibrator.setInitialMxy(0.0));
29253 assertThrows(LockedException.class, () -> calibrator.setInitialMxz(0.0));
29254 assertThrows(LockedException.class, () -> calibrator.setInitialMyx(0.0));
29255 assertThrows(LockedException.class, () -> calibrator.setInitialMyz(0.0));
29256 assertThrows(LockedException.class, () -> calibrator.setInitialMzx(0.0));
29257 assertThrows(LockedException.class, () -> calibrator.setInitialMzy(0.0));
29258 assertThrows(LockedException.class, () -> calibrator.setInitialScalingFactors(
29259 0.0, 0.0, 0.0));
29260 assertThrows(LockedException.class, () -> calibrator.setInitialCrossCouplingErrors(
29261 0.0, 0.0, 0.0, 0.0, 0.0, 0.0));
29262 assertThrows(LockedException.class, () -> calibrator.setInitialScalingFactorsAndCrossCouplingErrors(
29263 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
29264 0.0, 0.0, 0.0));
29265 assertThrows(LockedException.class, () -> calibrator.setBias((double[]) null));
29266 assertThrows(LockedException.class, () -> calibrator.setBias((Matrix) null));
29267 assertThrows(LockedException.class, () -> calibrator.setInitialMa(null));
29268 assertThrows(LockedException.class, () -> calibrator.setGroundTruthGravityNorm(0.0));
29269 assertThrows(LockedException.class, () -> calibrator.setGroundTruthGravityNorm((Acceleration) null));
29270 assertThrows(LockedException.class, () -> calibrator.setMeasurements(null));
29271 assertThrows(LockedException.class, () -> calibrator.setCommonAxisUsed(true));
29272 assertThrows(LockedException.class, () -> calibrator.setListener(this));
29273 assertThrows(LockedException.class, calibrator::calibrate);
29274 }
29275
29276 private static void assertEstimatedResult(
29277 final Matrix ma, final KnownBiasAndGravityNormAccelerometerCalibrator calibrator) {
29278
29279 assertEquals(ma.getElementAt(0, 0), calibrator.getEstimatedSx(), 0.0);
29280 assertEquals(ma.getElementAt(1, 1), calibrator.getEstimatedSy(), 0.0);
29281 assertEquals(ma.getElementAt(2, 2), calibrator.getEstimatedSz(), 0.0);
29282 assertEquals(ma.getElementAt(0, 1), calibrator.getEstimatedMxy(), 0.0);
29283 assertEquals(ma.getElementAt(0, 2), calibrator.getEstimatedMxz(), 0.0);
29284 assertEquals(ma.getElementAt(1, 0), calibrator.getEstimatedMyx(), 0.0);
29285 assertEquals(ma.getElementAt(1, 2), calibrator.getEstimatedMyz(), 0.0);
29286 assertEquals(ma.getElementAt(2, 0), calibrator.getEstimatedMzx(), 0.0);
29287 assertEquals(ma.getElementAt(2, 1), calibrator.getEstimatedMzy(), 0.0);
29288 }
29289
29290 private static void checkCommonAxisCovariance(final Matrix covariance) {
29291 assertEquals(9, covariance.getRows());
29292 assertEquals(9, covariance.getColumns());
29293
29294 for (var j = 0; j < 9; j++) {
29295 final var colIsZero = j == 5 || j == 7 || j == 8;
29296 for (var i = 0; i < 9; i++) {
29297 final var rowIsZero = i == 5 || i == 7 || i == 8;
29298 if (colIsZero || rowIsZero) {
29299 assertEquals(0.0, covariance.getElementAt(i, j), 0.0);
29300 }
29301 }
29302 }
29303 }
29304
29305 private static void checkGeneralCovariance(final Matrix covariance) {
29306 assertEquals(9, covariance.getRows());
29307 assertEquals(9, covariance.getColumns());
29308
29309 for (var i = 0; i < 9; i++) {
29310 assertNotEquals(0.0, covariance.getElementAt(i, i));
29311 }
29312 }
29313
29314 private static Matrix generateBa() {
29315 return Matrix.newFromArray(new double[]{
29316 900 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
29317 -1000 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
29318 800 * MICRO_G_TO_METERS_PER_SECOND_SQUARED});
29319 }
29320
29321 private static Matrix generateBg() {
29322 return Matrix.newFromArray(new double[]{
29323 -9 * DEG_TO_RAD / 3600.0,
29324 10 * DEG_TO_RAD / 3600.0,
29325 -8 * DEG_TO_RAD / 3600.0});
29326 }
29327
29328 private static Matrix generateMaGeneral() throws WrongSizeException {
29329 final var result = new Matrix(3, 3);
29330 result.fromArray(new double[]{
29331 500e-6, -300e-6, 200e-6,
29332 -150e-6, -600e-6, 250e-6,
29333 -250e-6, 70e-6, 450e-6
29334 }, false);
29335
29336 return result;
29337 }
29338
29339 private static Matrix generateMaCommonAxis() throws WrongSizeException {
29340 final var result = new Matrix(3, 3);
29341 result.fromArray(new double[]{
29342 500e-6, -300e-6, 200e-6,
29343 0.0, -600e-6, 250e-6,
29344 0.0, 0.0, 450e-6
29345 }, false);
29346
29347 return result;
29348 }
29349
29350 private static Matrix generateMg() throws WrongSizeException {
29351 final var result = new Matrix(3, 3);
29352 result.fromArray(new double[]{
29353 400e-6, -300e-6, 250e-6,
29354 0.0, -300e-6, -150e-6,
29355 0.0, 0.0, -350e-6
29356 }, false);
29357
29358 return result;
29359 }
29360
29361 private static Matrix generateGg() throws WrongSizeException {
29362 final var result = new Matrix(3, 3);
29363 final var tmp = DEG_TO_RAD / (3600 * 9.80665);
29364 result.fromArray(new double[]{
29365 0.9 * tmp, -1.1 * tmp, -0.6 * tmp,
29366 -0.5 * tmp, 1.9 * tmp, -1.6 * tmp,
29367 0.3 * tmp, 1.1 * tmp, -1.3 * tmp
29368 }, false);
29369
29370 return result;
29371 }
29372
29373 private static double getAccelNoiseRootPSD() {
29374 return 70.0 * MICRO_G_TO_METERS_PER_SECOND_SQUARED;
29375 }
29376
29377 private static double getGyroNoiseRootPSD() {
29378 return 0.01 * DEG_TO_RAD / 60.0;
29379 }
29380 }